// The code is open source under the MIT license.
// Copyright 2019-2020, Phillip Keldenich, TU Braunschweig, Algorithms Group
// https://ibr.cs.tu-bs.de/alg
//
// Permission is hereby granted, free of charge, to any person obtaining a copy of
// this software and associated documentation files (the "Software"), to deal in
// the Software without restriction, including without limitation the rights to
// use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
// of the Software, and to permit persons to whom the Software is furnished to do
// so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

//
// Created by Phillip Keldenich on 11.11.19.
//

#pragma once

#include "ivarp/number.hpp"
#include <boost/iterator/iterator_facade.hpp>
#include <cassert>

namespace ivarp {
    /// Computes split intervals for a given range and a fixed number of subdivisions n.
    /// Valid indices are [0,n).
    template<typename IntervalType> class Splitter {
    public:
        using Interval = IntervalType;
        using Number = typename Interval::NumberType;

        static_assert(IsIntervalType<IntervalType>::value, "Splitter requires an interval type to work on!");

        IVARP_SUPPRESS_HD
        explicit IVARP_HD Splitter(const IntervalType& i, int n) :
            m_range(i), m_n(n), m_ind_width((i.ub() - i.lb()) / n)
        {}

        IVARP_SUPPRESS_HD
        IVARP_HD Number split_point(int i) const {
            if(i >= m_n) {
                return m_range.ub();
            }
            return m_range.lb() + i * m_ind_width;
        }

        IVARP_SUPPRESS_HD
        IVARP_HD IntervalType subrange(int i) const {
            return IntervalType{split_point(i), split_point(i+1)};
        }

        IVARP_HD int size() const noexcept {
            return m_n;
        }

        class Iterator :
            public boost::iterator_facade<Iterator, IntervalType, std::random_access_iterator_tag, IntervalType, int>
        {
        public:
            Iterator() noexcept : m_splitter(nullptr), i(0) {}
            Iterator(const Iterator&) noexcept = default;
            Iterator &operator=(const Iterator&) noexcept = default;

        private:
            explicit Iterator(const Splitter* s, int i) :
                m_splitter(s), i(i)
            {}

            friend class Splitter;
            friend class boost::iterator_core_access;

            IntervalType dereference() const {
                return m_splitter->subrange(i);
            }

            void increment() noexcept {
                ++i;
            }

            void decrement() noexcept {
                --i;
            }

            void advance(int n) noexcept {
                i += n;
            }

            int distance_to(const Iterator& o) const noexcept {
                return o.i - i;
            }

            bool equal(const Iterator& o) const noexcept {
                return i == o.i;
            }

            const Splitter* m_splitter;
            int i;
        };

        Iterator begin() const noexcept {
            return Iterator{this, 0};
        }

        Iterator end() const noexcept {
            return Iterator{this, m_n};
        }

    private:
        IntervalType m_range; ///< The outer range.
        int m_n; ///< The number of subintervals.
        Number m_ind_width; ///< The width of each individual subinterval.
    };
}


/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2017 Hidekazu Ikeno
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

///
/// \file balanced_truncation.hpp
///
/// Balanced truncation method specialized for exponential sum function
///
#ifndef MXPFIT_BALANCED_TRUNCATION_HPP
#define MXPFIT_BALANCED_TRUNCATION_HPP
#include <cassert>

#include <Eigen/Core>
#include <Eigen/Eigenvalues>

#include <mxpfit/exponential_sum.hpp>
#include <mxpfit/quasi_cauchy_rrd.hpp>
#include <mxpfit/self_adjoint_coneigensolver.hpp>

namespace mxpfit
{

///
/// ### BalancedTruncation
///
/// \brief Find a truncated exponential sum function with smaller number of
///        terms by the modified balanced truncation method.
///
/// \tparam T  Scalar type of exponential sum function.
///
/// For a given exponential sum function,
///
/// \f[
///   f(t)=\sum_{j=1}^{n} c_{j}^{} e^{-a_{j}^{} t}, \quad
///   (\mathrm{Re}(a_{j}) > 0),
/// \f]
///
/// and prescribed accuracy \f$\epsilon > 0,\f$ this class calculates truncated
/// exponential \f$\hat{f}(t)\f$ sum such that
///
/// \f[
///   \hat{f}(t)=\sum_{j=1}^{k} \hat{c}_{j}^{}e^{-\hat{a}_{j}^{} t}, \quad
///   \left| f(t)-\hat{f}(t) \right| < \epsilon,
/// \f]
///
/// where \f$k \leq n.\f$ Let \f$F(s)\f$ and \f$\hat{F}(s)\f$ be the Laplace
/// transform of \f$f(t)\f$ and \f$\hat{f}(t),\f$ respectively. \f$F(s)\f$ can
/// be evaluated analytically as
///
/// \f[
///  F(s)=\sum_{j=1}^{n}\frac{c_{j}^{}}{s+a_{j}^{}}
/// \f]
///
/// and similar to \f$\hat{F}(s).\f$ Now, the problem can be rewritten as
/// finding optimal rational sum approximation \f$\hat{F}(s)\f$ such that \f$
/// \left|F(s)-\hat{F}(s)\right| < \epsilon.\f$
///
/// This class computes the truncated rational sum approximation
/// \f$\hat{F}(s)\f$ by the modified balanced truncation method combined with
/// the first and accurate con-eigensolver of a quasi-Cauchy matrix.
///
///
/// #### References
///
/// 1. K. Xu and S. Jiang, "A Bootstrap Method for Sum-of-Poles Approximations",
///    J. Sci. Comput. **55** (2013) 16-39.
///    [DOI: https://doi.org/10.1007/s10915-012-9620-9]
/// 2. T. S. Haut and G. Beylkin, "FAST AND ACCURATE CON-EIGENVALUE ALGORITHM
///    FOR OPTIMAL RATIONAL APPROXIMATIONS", SIAM J. Matrix Anal. Appl. **33**
///    (2012) 1101-1125.
///    [DOI: https://doi.org/10.1137/110821901]
/// 3. W. H. A. Schilders, H. A. van der Vorst, and J. Rommes, "Model Order
///    Reduction: Theory, Research Aspects and Applications", Springer (2008).
///    [DOI: https://doi.org/10.1007/978-3-540-78841-6]
///

template <typename T>
class BalancedTruncation
{
public:
    using Scalar        = T;
    using RealScalar    = typename Eigen::NumTraits<Scalar>::Real;
    using ComplexScalar = std::complex<RealScalar>;
    using Index         = Eigen::Index;

    using VectorType = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using MatrixType = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using ResultType = ExponentialSum<Scalar>;

    ///
    /// Compute truncated exponential sum \f$ \hat{f}(t) \f$
    ///
    /// \tparam DerivedF type of exponential sum inheriting ExponentialSumBase
    ///
    /// \param[in] orig original exponential sum function, \f$ f(t) \f$
    /// \param[in] threshold  prescribed accuracy \f$0 < \epsilon \ll 1\f$
    ///
    /// \return An instance of ExponentialSum represents \f$\hat{f}(t)\f$
    ///
    template <typename DerivedF>
    ResultType compute(const ExponentialSumBase<DerivedF>& orig,
                       RealScalar threshold);

private:
    enum
    {
        IsComplex = Eigen::NumTraits<Scalar>::IsComplex,
    };

    using EigenSolverType = typename Eigen::internal::conditional<
        IsComplex, Eigen::ComplexEigenSolver<MatrixType>,
        Eigen::SelfAdjointEigenSolver<MatrixType>>::type;
    using ConeigenSolverType = SelfAdjointConeigenSolver<Scalar>;
};

template <typename T>
template <typename DerivedF>
typename BalancedTruncation<T>::ResultType
BalancedTruncation<T>::compute(const ExponentialSumBase<DerivedF>& fn,
                               RealScalar threshold)
{
    //--------------------------------------------------------------------------
    //
    // The controllability Gramian matrix of the system is defined as
    //
    //   C(i, j) = sqrt(w[i] * conj(w[j])) / (p[i] + p[j]).
    //
    // C is a quasi-Cauchy matrix. Then compute partial Cholesky factorization
    // of matrix `C`
    //
    //   C = (P * L) * D^2 * (P * L)^H,
    //
    // where
    //
    //   - L: (n, m) matrix
    //   - D: (m, m) real diagonal matrix
    //   - P: (n, n) permutation matrix
    //
    // and `m = rank(C)`
    //
    //--------------------------------------------------------------------------
    static const RealScalar eps = Eigen::NumTraits<RealScalar>::epsilon();

    const Index n0 = fn.size();
    VectorType b(n0);
    b.array() = fn.weights().sqrt();

    const RealScalar rrd_threshold = threshold * eps * eps;
    SelfAdjointQuasiCauchyRRD<T> rrd;
    rrd.setThreshold(rrd_threshold);
    rrd.compute(b, fn.exponents());

    if (rrd.rank() == Index(0))
    {
        return ResultType();
    }

    //--------------------------------------------------------------------------
    //
    // Compute con-eigendecomposition of the controllability Gramian matrix
    //
    //   C = X D^2 X^H = U^C S U^T,
    //
    // where
    //
    //   - `X = P * L`: Cholesky factor (n, k)
    //   - `S`: (k, k) diagonal matrix. Diagonal elements `S(i,i)` are
    //          con-eigenvalues sorted in decreasing order.
    //   - `U`: (n, k) matrix. k-th column hold a con-eigenvector corresponding
    //          to k-th con-eigenvalue. The columns of `U` are orthogonal in the
    //          sense that `U^T * U = I`.
    //
    //---------------------------------------------------------------------------
    //
    // `diag` is overwritten by con-eigenvalues, and first k column of `matX` is
    // overwritten by con-eigenvectors `U`
    //
    ConeigenSolverType ceig;
    ceig.compute(rrd.matrixPL(), rrd.vectorD());
    //--------------------------------------------------------------------------
    //
    // Truncation
    //
    // Determines the order of reduced system, \f$ k \f$ from the error bound
    // computed from the Hankel singular values system. The Hankel singular
    // values are coincide with con-eigenvalues of the Gramian matrix.
    //
    // \f[
    //   \|\Sigma-\hat{\Sigma}\| \leq 2 \sum_{i=k+1}^{n} \sigma_{i}
    // \f]
    //
    //--------------------------------------------------------------------------
    auto sum_sigma       = RealScalar();
    const auto& sigma    = ceig.coneigenvalues();
    const auto sigma_tol = threshold * sigma(0);
    Index n1             = sigma.size();
    while (n1)
    {
        sum_sigma += sigma(n1 - 1);
        if (2 * sum_sigma > sigma_tol)
        {
            break;
        }
        --n1;
    }

    if (n1 == Index())
    {
        return ResultType();
    }

    //--------------------------------------------------------------------------
    //
    // Apply transformation matrix
    //
    //  A1 = U.adjoint() * S * U.conjugate()
    //  b1 = U.adjoint() * b
    //
    //--------------------------------------------------------------------------

    MatrixType A1(n1, n1);
    VectorType b1(n1);
    auto U = ceig.coneigenvectors().leftCols(n1);
    A1.noalias() =
        U.adjoint() * fn.exponents().matrix().asDiagonal() * U.conjugate();
    b1.noalias() = U.adjoint() * b;

    //--------------------------------------------------------------------------
    //
    // Compute eigenvalue decomposition of the (k x k) matrix, A1. Since A1
    // real/complex symmetric matrix, the eigen decomposition has the form
    //
    //   A1 = X2 * D * X2.transpose(), (X2.transpose() * X2 = I).
    //
    //--------------------------------------------------------------------------
    EigenSolverType eig(A1, Eigen::ComputeEigenvectors);

    if (IsComplex)
    {
        //
        // Enforce X2.transpose() * X2 = I
        //
        using EigenVectorsType = typename Eigen::internal::remove_all<decltype(
            eig.eigenvectors())>::type;
        auto& X2 = *const_cast<EigenVectorsType*>(&eig.eigenvectors());
        for (Index j = 0; j < X2.cols(); ++j)
        {
            auto xj          = X2.col(j);
            const auto t     = (xj.transpose() * xj).value();
            const auto scale = RealScalar(1) / std::sqrt(t);
            xj *= scale;
        }
    }

    //
    // Apply the state space transformation by X2,
    //
    // A2 = X2.transpose() * A1 * X2 = D
    // b2 = X2.transpose() * b1
    // c2 = b1 * X2 = b2.transpose()
    //
    // Finally parameters for truncated exponential sum can be obtained as
    //
    // p' = D.diagonal()
    // w' = c2 * b2 = square(b2)
    //
    auto b2      = b.head(n1);
    b2.noalias() = eig.eigenvectors().transpose() * b1;
    return ResultType(eig.eigenvalues(), b2.array().square());
}

} // namespace: mxpfit

#endif /* MXPFIT_BALANCED_TRUNCATION_HPP */


/**
 * @file MPCExample.cpp
 * @author Giulio Romualdi
 * @copyright Released under the terms of the BSD 3-Clause License
 * @date 2018
 */

// osqp-eigen
#include "OsqpEigen/OsqpEigen.h"

// eigen
#include <Eigen/Dense>

#include <iostream>
#include <fstream>

void setDynamicsMatrices(Eigen::Matrix<double, 2, 2> &a, Eigen::Matrix<double, 2, 1> &b)
{
    a << 1.,      0.020,
        0.,      0.9661;

    b << 0.,
        0.0315;
}

void setInequalityConstraints(Eigen::Matrix<double, 2, 1> &xMax, Eigen::Matrix<double, 2, 1> &xMin,
                              Eigen::Matrix<double, 1, 1> &uMax, Eigen::Matrix<double, 1, 1> &uMin)
{
    double u0 = 0.0;

    // input inequality constraints
    uMin << -6.0 - u0;

    uMax << 10.0 - u0;

    // state inequality constraints
    // TODO : change to present pos +/- ranges
    xMin << -100, -6.0;

    xMax << 100, 10.0;
}

void setWeightMatrices(Eigen::DiagonalMatrix<double, 2> &Q, Eigen::DiagonalMatrix<double, 1> &R)
{
    Q.diagonal() << 2, 0;
    R.diagonal() << 0.2;
}

void castMPCToQPHessian(const Eigen::DiagonalMatrix<double, 2> &Q, const Eigen::DiagonalMatrix<double, 1> &R, int mpcWindow,
                        Eigen::SparseMatrix<double> &hessianMatrix, int Nx, int Nu)
{

    hessianMatrix.resize(Nx*(mpcWindow+1) + Nu * mpcWindow, Nx*(mpcWindow+1) + Nu * mpcWindow);

    //populate hessian matrix
    for(int i = 0; i<Nx*(mpcWindow+1) + Nu * mpcWindow; i++){
        if(i < Nx*(mpcWindow+1)){
            int posQ=i%Nx;
            float value = Q.diagonal()[posQ];
            if(value != 0)
                hessianMatrix.insert(i,i) = value;
        }
        else{
            int posR=i%Nu;
            float value = R.diagonal()[posR];
            if(value != 0)
                hessianMatrix.insert(i,i) = value;
        }
    }
}

void castMPCToQPGradient(const Eigen::DiagonalMatrix<double, 2> &Q, const Eigen::Matrix<double, 2, 1> &xRef, int mpcWindow,
                         Eigen::VectorXd &gradient, int Nx, int Nu)
{

    Eigen::Matrix<double,2,1> Qx_ref;
    Qx_ref = Q * (-xRef);

    // populate the gradient vector
    gradient = Eigen::VectorXd::Zero(Nx*(mpcWindow+1) +  Nu*mpcWindow, 1);
    for(int i = 0; i<Nx*(mpcWindow+1); i++){
        int posQ=i%Nx;
        float value = Qx_ref(posQ,0);
        gradient(i,0) = value;
    }
}

void castMPCToQPConstraintMatrix(const Eigen::Matrix<double, 2, 2> &dynamicMatrix, const Eigen::Matrix<double, 2, 1> &controlMatrix,
                                 int mpcWindow, Eigen::SparseMatrix<double> &constraintMatrix, int Nx, int Nu)
{
    constraintMatrix.resize(Nx*(mpcWindow+1)  + Nx*(mpcWindow+1) + Nu * mpcWindow, Nx*(mpcWindow+1) + Nu * mpcWindow);

    // populate linear constraint matrix
    for(int i = 0; i<Nx*(mpcWindow+1); i++){
        constraintMatrix.insert(i,i) = -1;
    }

    for(int i = 0; i < mpcWindow; i++)
        for(int j = 0; j<Nx; j++)
            for(int k = 0; k<Nx; k++){
                float value = dynamicMatrix(j,k);
                if(value != 0){
                    constraintMatrix.insert(Nx * (i+1) + j, Nx * i + k) = value;
                }
            }

    for(int i = 0; i < mpcWindow; i++)
        for(int j = 0; j < Nx; j++)
            for(int k = 0; k < Nu; k++){
                float value = controlMatrix(j,k);
                if(value != 0){
                    constraintMatrix.insert(Nx*(i+1)+j, Nu*i+k+Nx*(mpcWindow + 1)) = value;
                }
            }

    for(int i = 0; i<Nx*(mpcWindow+1) + Nu*mpcWindow; i++){
        constraintMatrix.insert(i+(mpcWindow+1)*Nx,i) = 1;
    }
}

void castMPCToQPConstraintVectors(const Eigen::Matrix<double, 2, 1> &xMax, const Eigen::Matrix<double, 2, 1> &xMin,
                                   const Eigen::Matrix<double, 1, 1> &uMax, const Eigen::Matrix<double, 1, 1> &uMin,
                                   const Eigen::Matrix<double, 2, 1> &x0,
                                   int mpcWindow, Eigen::VectorXd &lowerBound, Eigen::VectorXd &upperBound, int Nx, int Nu)
{
    // evaluate the lower and the upper inequality vectors
    Eigen::VectorXd lowerInequality = Eigen::MatrixXd::Zero(Nx*(mpcWindow+1) +  Nu * mpcWindow, 1);
    Eigen::VectorXd upperInequality = Eigen::MatrixXd::Zero(Nx*(mpcWindow+1) +  Nu * mpcWindow, 1);
    for(int i=0; i<mpcWindow+1; i++){
        lowerInequality.block(Nx*i,0,Nx,1) = xMin;
        upperInequality.block(Nx*i,0,Nx,1) = xMax;
    }
    for(int i=0; i<mpcWindow; i++){
        lowerInequality.block(Nu * i + Nx * (mpcWindow + 1), 0, Nu, 1) = uMin;
        upperInequality.block(Nu * i + Nx * (mpcWindow + 1), 0, Nu, 1) = uMax;
    }

    // evaluate the lower and the upper equality vectors
    Eigen::VectorXd lowerEquality = Eigen::MatrixXd::Zero(Nx*(mpcWindow+1),1 );
    Eigen::VectorXd upperEquality;
    lowerEquality.block(0,0,Nx,1) = -x0;
    upperEquality = lowerEquality;
    lowerEquality = lowerEquality;

    // merge inequality and equality vectors
    lowerBound = Eigen::MatrixXd::Zero(2*Nx*(mpcWindow+1) +  Nu*mpcWindow,1 );
    lowerBound << lowerEquality,
        lowerInequality;

    upperBound = Eigen::MatrixXd::Zero(2*Nx*(mpcWindow+1) +  Nu*mpcWindow,1 );
    upperBound << upperEquality,
        upperInequality;
}

void updateConstraintVectors(const Eigen::Matrix<double, 2, 1> &x0,
                             Eigen::VectorXd &lowerBound, Eigen::VectorXd &upperBound, int Nx)
{
    lowerBound.block(0,0,Nx,1) = -x0;
    upperBound.block(0,0,Nx,1) = -x0;
}

double getErrorNorm(const Eigen::Matrix<double, 2, 1> &x,
                    const Eigen::Matrix<double, 2, 1> &xRef, int Nx)
{
    // evaluate the error
    Eigen::Matrix<double, 2, 1> error = x - xRef;

    // return the norm
    return error.norm();
}

int main()
{
    // set the preview window
    int mpcWindow = 200;
    const int Nx = 2;
    const int Nu = 1;

    // allocate the dynamics matrices
    Eigen::Matrix<double, Nx, Nx> a;
    Eigen::Matrix<double, Nx, Nu> b;

    // allocate the constraints vector
    Eigen::Matrix<double, Nx, 1> xMax;
    Eigen::Matrix<double, Nx, 1> xMin;
    Eigen::Matrix<double, Nu, 1> uMax;
    Eigen::Matrix<double, Nu, 1> uMin;

    // allocate the weight matrices
    Eigen::DiagonalMatrix<double, Nx> Q;
    Eigen::DiagonalMatrix<double, Nu> R;

    // allocate the initial and the reference state space
    Eigen::Matrix<double, Nx, 1> x0;
    Eigen::Matrix<double, Nx, 1> xRef;

    // allocate QP problem matrices and vectores
    Eigen::SparseMatrix<double> hessian;
    Eigen::VectorXd gradient;
    Eigen::SparseMatrix<double> linearMatrix;
    Eigen::VectorXd lowerBound;
    Eigen::VectorXd upperBound;

    // set the initial and the desired states
    x0 << 0, 0 ;
    xRef <<  1/0.127, 0;

    // set MPC problem quantities
    setDynamicsMatrices(a, b);
    setInequalityConstraints(xMax, xMin, uMax, uMin);
    setWeightMatrices(Q, R);

    // cast the MPC problem as QP problem
    castMPCToQPHessian(Q, R, mpcWindow, hessian, Nx, Nu);
    castMPCToQPGradient(Q, xRef, mpcWindow, gradient, Nx, Nu);
    castMPCToQPConstraintMatrix(a, b, mpcWindow, linearMatrix, Nx, Nu);
    castMPCToQPConstraintVectors(xMax, xMin, uMax, uMin, x0, mpcWindow, lowerBound, upperBound, Nx, Nu);

    // instantiate the solver
    OsqpEigen::Solver solver;

    // settings
    //solver.settings()->setVerbosity(false);
    solver.settings()->setWarmStart(true);

    // set the initial data of the QP solver
    solver.data()->setNumberOfVariables(Nx * (mpcWindow + 1) + Nu * mpcWindow);
    solver.data()->setNumberOfConstraints(2 * Nx * (mpcWindow + 1) + Nu * mpcWindow);
    if(!solver.data()->setHessianMatrix(hessian)) return 1;
    if(!solver.data()->setGradient(gradient)) return 1;
    if(!solver.data()->setLinearConstraintsMatrix(linearMatrix)) return 1;
    if(!solver.data()->setLowerBound(lowerBound)) return 1;
    if(!solver.data()->setUpperBound(upperBound)) return 1;

    // instantiate the solver
    if(!solver.initSolver()) return 1;

    // controller input and QPSolution vector
    Eigen::VectorXd ctr;
    Eigen::VectorXd QPSolution;

    // number of iteration steps
    int numberOfSteps = 200;

    std::ofstream myfile;
    myfile.open ("mpc_log.csv");
    myfile << "Log of MPC by osqp in C++.\n";
    myfile << "x1,x2,\n";

    for (int i = 0; i < numberOfSteps; i++){

        // solve the QP problem
        if(solver.solveProblem() != OsqpEigen::ErrorExitFlag::NoError) return 1;

        // get the controller input
        QPSolution = solver.getSolution();
        ctr = QPSolution.block(Nx * (mpcWindow + 1), 0, Nu, 1);

        // save data into file
        auto x0Data = x0.data();

        // propagate the model
        x0 = a * x0 + b * ctr;
        myfile << x0[0] << "," << x0[1] <<",\n";

        // update the constraint bound
        updateConstraintVectors(x0, lowerBound, upperBound, Nx);
        if(!solver.updateBounds(lowerBound, upperBound)) return 1;
      }
    myfile.close();
    return 0;
}


#pragma once
#include <Eigen/Core>
#include "datatypes.hpp"
#include <Eigen/Eigen>
#include "profile.hpp"
#include "integration.hpp"
#include "transformation.hpp"

namespace gd {

using namespace std;
USING_PART_OF_NAMESPACE_EIGEN

class Grid1d {
public:
	virtual int findindex(double) { return 0; }
	virtual double indexto_x(int) { return 0; }
	//virtual void indexto_x(int) { return 0; }
	virtual bool inrange(double) { return false; }
	virtual int length() {return 0;}
};

template<class Base=Grid1d>
class Grid1dRegular : public Base {
public:
	Grid1dRegular(double x1, double x2, int gridpoints) : x1(x1), x2(x2), _gridpoints(gridpoints) {}
	int findindex(double x) {
		return x < x1 ? 0 :
			(x >= x2 ? _gridpoints-1 : (int)((x-x1)/(x2-x1)*(_gridpoints-1)));
	}
	double indexto_x(int i) {
		return x1 + (x2-x1)/(length()-1)*i;
	}
	bool inrange(double r) {
		return (r >= x1) && (r < x2);
	}
	int length() { return _gridpoints; }
	double x1, x2;
	int _gridpoints;
};

/*class Grid2d {
public:
	virtual int findindex(double, double) { return 0; }
	virtual bool inrange(double, double) { return false; }
	virtual int length() {return 0;}
};*/

template<class GridX=Grid1d, class GridY=Grid1d>
class Grid2d {
public:
	typedef GridX GridXType;
	typedef GridY GridYType;
	Grid2d(GridX* gridx, GridY* gridy) : gridx(gridx), gridy(gridy) {}
	void findindex2d(double x, double y, int& xi, int &yi) {
		//double r = sqrt(x*x+y*y);
		//double phi = fmod(atan2(y,x)+2*M_PI, 2*M_PI);
		xi = gridx->findindex(x);
		yi = gridy->findindex(y);
	}
	int findindex(double x, double y) {
		//double r = sqrt(x*x+y*y);
		//double phi = fmod(atan2(y,x)+2*M_PI, 2*M_PI);
		return gridx->findindex(x) * gridy->length() + gridy->findindex(y);;
	}
	void indexto_xy(int i, int j, double &x, double &y) {
		x = gridx->indexto_x(i);
		y = gridy->indexto_x(j);
	}
	bool inrange(double x, double y) {
		//double r = sqrt(x*x+y*y);
		//double phi = fmod(atan2(y,x)+2*M_PI, 2*M_PI);
		return gridx->inrange(x) && gridy->inrange(y);
	}
	int length() {
		return gridx->length() * gridy->length();
	}
	GridX* gridx;
	GridY* gridy;
};

template<class GridX=Grid1d, class GridY=Grid1d, class GridZ=Grid1d>
class Grid3d {
public:
	typedef GridX GridXType;
	typedef GridY GridYType;
	typedef GridZ GridZType;
	Grid3d(GridX* gridx, GridY* gridy, GridZ* gridz) : gridx(gridx), gridy(gridy), gridz(gridz) {}
	void findindex3d(double x, double y, double z, int& xi, int &yi, int &zi) {
		//double r = sqrt(x*x+y*y);
		//double phi = fmod(atan2(y,x)+2*M_PI, 2*M_PI);
		xi = gridx->findindex(x);
		yi = gridy->findindex(y);
		zi = gridz->findindex(z);
	}
	int findindex(double x, double y, double z) {
		//double r = sqrt(x*x+y*y);
		//double phi = fmod(atan2(y,x)+2*M_PI, 2*M_PI);
		return (gridx->findindex(x) * gridy->length() + gridy->findindex(y)) * gridz->length() + gridz->findindex(z);
	}
	void indexto_xyz(int i, int j, int k, double &x, double &y, double &z) {
		x = gridx->indexto_x(i);
		y = gridy->indexto_x(j);
		z = gridz->indexto_x(k);
	}
	bool inrange(double x, double y, double z) {
		//double r = sqrt(x*x+y*y);
		//double phi = fmod(atan2(y,x)+2*M_PI, 2*M_PI);
		return gridx->inrange(x) && gridy->inrange(y) && gridz->inrange(z);
	}
	int length() {
		return gridx->length() * gridy->length() * gridz->length();
	}
	GridX* gridx;
	GridY* gridy;
	GridZ* gridz;
};

/*
		basis_evaluator<2, Basis::degree, Basis> basis_evaluator;
		if(mesh.inrange(x))
			value = basis_evaluator.eval(solution, this, xi, u, yi, v);
*/

template<int DIM, class B, int I=B::degree, class BI=B, class T=double>
struct basis_evaluator;

template<int DIM, class B, int I, class BI, class T>
struct basis_evaluator
{
	typedef basis_evaluator<DIM, B, I-1, typename BI::next_type, T> next_typeI;
	typedef basis_evaluator<DIM-1, B, B::degree, B, T> next_typeDIM;
	next_typeI nextI;
	next_typeDIM nextDIM;

	template<class... Ts>
	T eval(int i, double u, Ts... ts) {
		if((i % (B::degree+1)) == I) { // first search for right i
			BI basis;
			return basis(u) * nextDIM.eval(i/(B::degree+1), ts...);
		} else {
			return nextI.eval(i, u, ts...);
		}
	}
};

template<int DIM, class B,  class BI, class T>
struct basis_evaluator<DIM, B, -1, BI, T>
{
	template<class... Ts>
	T eval(int, double, Ts...) {
		return 1;
	}
};

template<class B, int I, class BI, class T>
struct basis_evaluator<0, B, I, BI, T>
{
	T eval(int) {
		return 1;
	}
};

template<int DIM, int I, class B, class BI, class T>
struct basis_function_integrator;

//template<int DIM, int I, class B, class BI, class T>
//struct basis_function_integrator<2,I,B,BI,T>
template<int DIM, int I, class B, class BI, class T>
struct basis_function_integrator
{
	typedef basis_function_integrator<DIM,   I-1, B, typename BI::next_type, T> next_typeI;
	typedef basis_function_integrator<DIM-1, B::degree, B, B, T> next_typeDIM;
	T x1, x2;
	next_typeI nextI;
	next_typeDIM nextDIM;

	template<class... Ts>
	basis_function_integrator(T x1, T x2, Ts... ts) : x1(x1), x2(x2), nextI(x1, x2, ts...), nextDIM(ts...) {
		//cout << "DIM " << DIM << " " << x1 << " " << x2 << endl;
	}

	template<class F>
	T integrate_function2d(F f, int i) {
		if((i % (B::degree+1)) == I) { // first search for right i
			BI basis;
			double dx = (x2-x1);
			auto fi = [&](double x) -> double { return basis((x-this->x1)/dx) / dx  * this->nextDIM.integrate_function2d_y(f, i / (B::degree+1), x); };
			IntegratorGSL<> integratorGSL(fi);
			//cout << "integrate: " << x1 << " to " << x2 << endl;
			return integratorGSL.integrate(x1, x2);
		} else {
			return nextI.integrate_function2d(f, i);
		}
	}
	template<class F>
	T integrate_function2d_y(F f, int i, double x) {
		if((i % (B::degree+1)) == I) { // first search for right i
			BI basis;
			double dx = (x2-x1);
			auto fi = [&](double y) -> double { return basis((y-this->x1)/dx)/dx * f(x, y); };
			IntegratorGSL<> integratorGSL(fi);
			//cout << "integrate: " << x1 << " to " << x2 << endl;
			return integratorGSL.integrate(x1, x2);
		} else {
			return nextI.integrate_function2d_y(f, i, x);
		}
	}
};

template<int DIM, class B, class BI, class T>
struct basis_function_integrator<DIM, -1, B, BI, T>
{
	template<class... Ts>
	basis_function_integrator(T, T, Ts...) {}
	template<class F>
	T integrate_function2d(F, int) { return 1; }
	template<class F>
	T integrate_function2d_y(F, int, double) { return 1; }
};

template<int I, class B, class BI, class T>
struct basis_function_integrator<0, I, B, BI, T>
{
	template<class F>
	T integrate_function(F, int) { return 1; }
};

template<int DIM, int I, int J, class B, class BI, class BJ, class T=double>
struct basis_integrator;

template<int DIM, int I, int J, class B, class BI, class BJ, class T>
struct basis_integrator
{
	typedef basis_integrator<DIM, I-1, J, B, typename BI::next_type, BJ, T> next_typeI;
	typedef basis_integrator<DIM, I, J-1, B, BI, typename BJ::next_type, T> next_typeJ;
	typedef basis_integrator<DIM-1, B::degree, B::degree, B, B, B, T> next_typeDIM;
	next_typeI nextI;
	next_typeJ nextJ;
	next_typeDIM nextDIM;

	//template<class... Ts>
	T integrate(int i, int j) {
		if((i % (B::degree+1)) == I) { // first search for right i
			if((j  % (B::degree+1)) == J) { // then right j
				BI basis1;
				BJ basis2;
				auto f = [&](double x) ->double { return basis1(x) * basis2(x) * this->nextDIM.integrate(i / (B::degree+1), j / (B::degree+1)); };
				IntegratorGSL<> integratorGSL(f);
				return integratorGSL.integrate(0, 1);
			}  else {
				return nextJ.integrate(i, j);
			}
		} else {
			return nextI.integrate(i, j);
		}
	}
};

// sentinels
template<int I, int J, class B, class BI, class BJ, class T>
struct basis_integrator<0, I, J, B, BI, BJ, T>
{
	template<class... Ts>
	T integrate(int, int) {
		return 1;
	}
};

template<int DIM, int J, class B, class BI, class BJ, class T>
struct basis_integrator<DIM, -1, J, B, BI, BJ, T>
{
	template<class... Ts>
	T integrate(int, int) {
		return 1;
	}
};

template<int DIM, int I, class B, class BI, class BJ, class T>
struct basis_integrator<DIM, I, -1, B, BI, BJ, T>
{
	template<class... Ts>
	T integrate(int, int) {
		return 1;
	}
};

template<int DIM, class Basis, class T=double>
struct MeshRegularNodalHelper;

template<class Basis, class T>
struct MeshRegularNodalHelper<0, Basis, T> {
	typedef Basis basis_type;
	enum { dof_per_cell = 1 };
};

template<int DIM, class Basis, class T>
struct MeshRegularNodalHelper {
	typedef Basis basis_type;
	typedef MeshRegularNodalHelper<DIM, Basis, T> type;
	typedef MeshRegularNodalHelper<DIM-1, Basis, T> sub_type;
	enum { dof_per_cell = (Basis::degree+1) * sub_type::dof_per_cell };

	/*int get_dof() { return dof;}
	int get_n_cells() { return n_cells;}
	int dof_index(int cell_index, int local_index) {
		return  cell_index*(dof_per_cell-1)+local_index;
	}*/
};

template<int DIM, class Basis, class T=double>
class MeshRegularNodal;

template<class Basis, class T>
class MeshRegularNodal<2, Basis, T> {
public:
	typedef Basis basis_type;
	T x1, x2;
	int n_cells_x;
	int n_cells_y;
	Grid1dRegular<> xgrid;
	Grid1dRegular<> ygrid;
	Grid2d<Grid1dRegular<>, Grid1dRegular<>> grid;
	MatrixXd M;
	//Transformation1d_in_3d* transformation;
	int dof, dofx, dofy;
	int dof1d;
	enum { dof_per_cell = MeshRegularNodalHelper<2, Basis, T>::dof_per_cell };
	enum { dof_per_cell1d = MeshRegularNodalHelper<1, Basis, T>::dof_per_cell };

	MeshRegularNodal(T x1, T y1, T x2, T y2, int n_cells_x, int n_cells_y) : n_cells_x(n_cells_x), n_cells_y(n_cells_y), xgrid(x1, x2, n_cells_x+1), ygrid(y1, y2, n_cells_y+1), grid(&xgrid, &ygrid), M(1, 1) {

		if(Basis::degree == 0) {
			dofx = n_cells_x;
			dofy = n_cells_y;
		} else {
			dofx = (1 + n_cells_x) + (dof_per_cell1d-2)*n_cells_x; // 1 dof per border + dofs inside the cel
			dofy = (1 + n_cells_y) + (dof_per_cell1d-2)*n_cells_y; // 1 dof per border + dofs inside the cel
		}
		dof = dofx * dofy;
		//cout << "n_cells_x = " << n_cells_x << " dofx = " << dofx << endl;
		//cout << "n_cells_y = " << n_cells_y << " dofy = " << dofy << endl;
		//cout << "dof = " << dof << " dof_per_cell = " << dof_per_cell << endl;
		//MatrixXd m = MatrixXd::Zero(dof, dof);
		M.resize(dof, dof);
		M =  MatrixXd::Zero(dof, dof);
		//T scale = 1; //TODO: (x2-x1)/n_cells;

		//basis_integrator<1, Basis::degree, Basis::degree, Basis, Basis, Basis, T> bi;
		//cout  << "test 00 " << bi.integrate(0, 0) << endl;
		//cout  << "test 01 " << bi.integrate(0, 1) << endl;
		//cout  << "test 11 " << bi.integrate(1, 1) << endl;

		T integrals[dof_per_cell][dof_per_cell];
		for(int j = 0; j < dof_per_cell; j++) {
			for(int k = 0; k < (j+1); k++) {
				basis_integrator<2, Basis::degree, Basis::degree, Basis, Basis, Basis, T> bi;
				T integral = bi.integrate(j, k);
				//typedef selfintegrator<Basis::degree, Basis::degree> selfintegrator_type;
				//selfintegrator_type si;
				//double integral = si.integrate(j,k);
				//cout << j << " " << k << " " << integral << endl;
				integrals[j][k] = integral;
				integrals[k][j] = integral;
			}
		}
		for(int xi = 0; xi < n_cells_x; xi++) {
			for(int yi = 0; yi < n_cells_y; yi++) {
				for(int j = 0; j < dof_per_cell; j++) {
				for(int k = 0; k < dof_per_cell; k++) {
					int i1 = this->dof_index(xi, yi, j);
					int i2 = this->dof_index(xi, yi, k);
					//cout << "xi = " << xi << " yi = " << yi << " j = " << j << " k = " << k;
					//cout << "            i1 = " << i1 << " i2 = " << i2 << endl;
					M(i1, i2) += integrals[j][k];
				}}
			}
		}
		/*for(int i = 0; i < n_cells; i++) {
			for(int j = 0; j < dof_per_cell; j++) {
				for(int k = 0; k < dof_per_cell; k++) {
					int i1 = i*(dof_per_cell-1)+j;
					int i2 = i*(dof_per_cell-1)+k;
					m(i1, i2) = m(i1, i2) + integrals[j][k];
				}
			}
		}*/
		//cout << M << endl;
		//m = m * scale;
		//ctrans = m.inverse();
	}

	double eval_(double_vector solution_, double x, double y) {
		VectorXd solution = VectorXd::Map(solution_.data().begin(), solution_.size());
		return eval(solution, x, y);
	}

	double eval(VectorXd& solution, double x, double y) {
		/*int cell_index = mesh.findindex(x);
		T xleft = mesh.indexto_x(cell_index);
		T xright = mesh.indexto_x(cell_index+1);
		*/
		int xi = xgrid.findindex(x);
		int yi = ygrid.findindex(y);
		T x1 = xgrid.indexto_x(xi);
		T x2 = xgrid.indexto_x(xi+1);
		T y1 = ygrid.indexto_x(yi);
		T y2 = ygrid.indexto_x(yi+1);
		double u = (x-x1)/(x2-x1);
		double v = (y-y1)/(y2-y1);
		//grid.index
		//T dx = xright-xleft;
		double value = 0;
		//cout << "xi = " << xi;
		//cout << " yi = " << yi;
		if(grid.inrange(x, y)) {
			basis_evaluator<2, Basis> basis_evaluator;
			for(int i = 0; i < dof_per_cell; i++) {
				//cout << " i = " << i;
				//cout << " index = " << dof_index(xi, yi, i) << endl;
				double a = solution(dof_index(xi, yi, i));
				value += a * basis_evaluator.eval(i, u, v);
			}
		} else {
			cout << "x and y not in range: (" << x << "," << y << ")" << endl;
		}
		return value;
	}

	double basis_uv(int i, double u, double v) {
		basis_evaluator<2, Basis> basis_evaluator;
		return basis_evaluator.eval(i, u, v);
	}

	void solve_coordinates(double_vector inner_products, double_vector coordinates) {
		assert((int)inner_products.size() == dof);
		assert((int)coordinates.size() == dof);
		VectorXd x = VectorXd::Map(inner_products.data().begin(), inner_products.size());
		VectorXd a(dof);
		M.llt().solve(x, &a);
		VectorXd::Map(coordinates.data().begin(), coordinates.size()) = a;
	}

	void test(double_vector result_, double scale1, double scale2) {
		auto f = [&](double x, double y) -> double { return cos(x * scale1 + y * scale2); };
		VectorXd x = VectorXd::Zero(dof);
		assert((int)result_.size() == dof);
		for(int yi = 0; yi < n_cells_y; yi++) {
			for(int xi = 0; xi < n_cells_x; xi++) {
				for(int i = 0; i < dof_per_cell; i++) {
					double x1, x2;
					double y1, y2;
					grid.indexto_xy(xi, yi, x1, y1);
					grid.indexto_xy(xi+1, yi+1, x2, y2);
					//cout << "integrate x[" << x1 << " " << x2 << "] y[" << y1 << " " << y2 << "]";
					basis_function_integrator<2, Basis::degree, Basis, Basis, T> bfi(x1, x2, y1, y2);
					double a = bfi.integrate_function2d(f, i);
					x(dof_index(xi, yi, i)) += a;
					//cout << " " << a;
 					//cout << endl;
				}
			}
		}
		VectorXd a(dof);
		M.llt().solve(x, &a);
		//VectorXd result = VectorXd::Map(result_.data().begin(), result_.size());
		VectorXd::Map(result_.data().begin(), result_.size()) = a;
		//result = a;
	}

	//int get_dof() { return dof; }
	int dof_index(int x_index, int y_index, int basis_index) {
		if(Basis::degree == 0) {
			return x_index + y_index * dofx;
		}
		int xi = x_index*(dof_per_cell1d-1) + basis_index % dof_per_cell1d;
		int yi = y_index*(dof_per_cell1d-1) + basis_index / dof_per_cell1d;
		return xi + yi * dofx;
	}
	int get_dof() { return dof; }
	int get_dof_per_cell() { return dof_per_cell; }
	/*int get_n_cells() { return grid->;}
	int dof_index(int cell_index, int local_index) {
		return  cell_index*(dof_per_cell-1)+local_index;
	}*/

}; // class MeshRegularNodal<2...>

template<class Basis, class T>
class MeshRegularNodal<3, Basis, T> {
	public:
		typedef Basis basis_type;
		T x1, x2;
		int n_cells_x;
		int n_cells_y;
		int n_cells_z;
		Grid1dRegular<> xgrid;
		Grid1dRegular<> ygrid;
		Grid1dRegular<> zgrid;
		Grid3d<Grid1dRegular<>, Grid1dRegular<>, Grid1dRegular<>> grid;
		MatrixXd M;
	//Transformation1d_in_3d* transformation;
		int dof, dofx, dofy, dofz;
		int dof1d;
		enum { dof_per_cell = MeshRegularNodalHelper<3, Basis, T>::dof_per_cell };
		enum { dof_per_cell1d = MeshRegularNodalHelper<1, Basis, T>::dof_per_cell };

	MeshRegularNodal(T x1, T y1, T z1, T x2, T y2, T z2, int n_cells_x, int n_cells_y, int n_cells_z) : n_cells_x(n_cells_x), n_cells_y(n_cells_y), n_cells_z(n_cells_z), xgrid(x1, x2, n_cells_x+1), ygrid(y1, y2, n_cells_y+1), zgrid(z1, z2, n_cells_z+1), grid(&xgrid, &ygrid, &zgrid), M(1, 1) {

		if(Basis::degree == 0) {
			dofx = n_cells_x;
			dofy = n_cells_y;
			dofz = n_cells_z;
		} else {
			dofx = (1 + n_cells_x) + (dof_per_cell1d-2)*n_cells_x; // 1 dof per border + dofs inside the cel
			dofy = (1 + n_cells_y) + (dof_per_cell1d-2)*n_cells_y; // 1 dof per border + dofs inside the cel
			dofz = (1 + n_cells_z) + (dof_per_cell1d-2)*n_cells_z; // 1 dof per border + dofs inside the cel
		}
		dof = dofx * dofy * dofz;
		cout << "n_cells_x = " << n_cells_x << " dofx = " << dofx << endl;
		cout << "n_cells_y = " << n_cells_y << " dofy = " << dofy << endl;
		cout << "n_cells_z = " << n_cells_z << " dofz = " << dofz << endl;
		cout << "dof = " << dof << " dof_per_cell = " << dof_per_cell << endl;
		//MatrixXd m = MatrixXd::Zero(dof, dof);
		M.resize(dof, dof);
		M =  MatrixXd::Zero(dof, dof);
		//T scale = 1; //TODO: (x2-x1)/n_cells;

		//basis_integrator<1, Basis::degree, Basis::degree, Basis, Basis, Basis, T> bi;
		//cout  << "test 00 " << bi.integrate(0, 0) << endl;
		//cout  << "test 01 " << bi.integrate(0, 1) << endl;
		//cout  << "test 11 " << bi.integrate(1, 1) << endl;

		T integrals[dof_per_cell][dof_per_cell];
		for(int j = 0; j < dof_per_cell; j++) {
			for(int k = 0; k < (j+1); k++) {
				basis_integrator<2, Basis::degree, Basis::degree, Basis, Basis, Basis, T> bi;
				T integral = bi.integrate(j, k);
				//typedef selfintegrator<Basis::degree, Basis::degree> selfintegrator_type;
				//selfintegrator_type si;
				//double integral = si.integrate(j,k);
				//cout << j << " " << k << " " << integral << endl;
				integrals[j][k] = integral;
				integrals[k][j] = integral;
			}
		}
		for(int xi = 0; xi < n_cells_x; xi++) {
			for(int yi = 0; yi < n_cells_y; yi++) {
				for(int zi = 0; zi < n_cells_z; zi++) {
					for(int j = 0; j < dof_per_cell; j++) {
						for(int k = 0; k < dof_per_cell; k++) {
							int i1 = this->dof_index(xi, yi, zi, j);
							int i2 = this->dof_index(xi, yi, zi, k);
							//cout << "xi = " << xi << " yi = " << yi << " zi = " << zi << " j = " << j << " k = " << k;
							//cout << "            i1 = " << i1 << " i2 = " << i2 << endl;
							M(i1, i2) += integrals[j][k];
					}}
				}
			}
		}
		/*for(int i = 0; i < n_cells; i++) {
			for(int j = 0; j < dof_per_cell; j++) {
				for(int k = 0; k < dof_per_cell; k++) {
					int i1 = i*(dof_per_cell-1)+j;
					int i2 = i*(dof_per_cell-1)+k;
					m(i1, i2) = m(i1, i2) + integrals[j][k];
				}
			}
		}*/
		//cout << M << endl;
		//m = m * scale;
		//ctrans = m.inverse();
	}

		double eval_(double_vector solution_, double x, double y, double z) {
			VectorXd solution = VectorXd::Map(solution_.data().begin(), solution_.size());
			return eval(solution, x, y, z);
		}

		double eval(VectorXd& solution, double x, double y, double z) {
		/*int cell_index = mesh.findindex(x);
		T xleft = mesh.indexto_x(cell_index);
		T xright = mesh.indexto_x(cell_index+1);
		*/
			int xi = xgrid.findindex(x);
			int yi = ygrid.findindex(y);
			int zi = zgrid.findindex(z);
			T x1 = xgrid.indexto_x(xi);
			T x2 = xgrid.indexto_x(xi+1);
			T y1 = ygrid.indexto_x(yi);
			T y2 = ygrid.indexto_x(yi+1);
			T z1 = zgrid.indexto_x(zi);
			T z2 = zgrid.indexto_x(zi+1);
			double u = (x-x1)/(x2-x1);
			double v = (y-y1)/(y2-y1);
			double w = (z-z1)/(z2-z1);
		//grid.index
		//T dx = xright-xleft;
			double value = 0;
		//cout << "xi = " << xi;
		//cout << " yi = " << yi;
			if(grid.inrange(x, y, z)) {
				basis_evaluator<3, Basis> basis_evaluator;
				for(int i = 0; i < dof_per_cell; i++) {
				//cout << " i = " << i;
				//cout << " index = " << dof_index(xi, yi, i) << endl;
					double a = solution(dof_index(xi, yi, zi, i));
					value += a * basis_evaluator.eval(i, u, v, w);
				}
			} else {
				cout << "x, y and z not in range: (" << x << "," << y << "," << z << ")" << endl;
			}
			return value;
		}

		double basis_uv(int i, double u, double v, double w) {
			basis_evaluator<3, Basis> basis_evaluator;
			return basis_evaluator.eval(i, u, v, w);
		}

		void solve_coordinates(double_vector inner_products, double_vector coordinates) {
			assert((int)inner_products.size() == dof);
			assert((int)coordinates.size() == dof);
			VectorXd x = VectorXd::Map(inner_products.data().begin(), inner_products.size());
			VectorXd a(dof);
			M.llt().solve(x, &a);
			VectorXd::Map(coordinates.data().begin(), coordinates.size()) = a;
		}

		/*void test(double_vector result_, double scale1, double scale2) {
			auto f = [&](double x, double y) -> double { return cos(x * scale1 + y * scale2); };
			VectorXd x = VectorXd::Zero(dof);
			assert(result_.size() == dof);
			for(int yi = 0; yi < n_cells_y; yi++) {
				for(int xi = 0; xi < n_cells_x; xi++) {
					for(int i = 0; i < dof_per_cell; i++) {
						double x1, x2;
						double y1, y2;
						grid.indexto_xy(xi, yi, x1, y1);
						grid.indexto_xy(xi+1, yi+1, x2, y2);
					//cout << "integrate x[" << x1 << " " << x2 << "] y[" << y1 << " " << y2 << "]";
						basis_function_integrator<2, Basis::degree, Basis, Basis, T> bfi(x1, x2, y1, y2);
						double a = bfi.integrate_function2d(f, i);
						x(dof_index(xi, yi, i)) += a;
					//cout << " " << a;
 					//cout << endl;
					}
				}
			}
			VectorXd a(dof);
			M.llt().solve(x, &a);
		//VectorXd result = VectorXd::Map(result_.data().begin(), result_.size());
			VectorXd::Map(result_.data().begin(), result_.size()) = a;
		//result = a;
		}*/

	//int get_dof() { return dof; }
		int dof_index(int x_index, int y_index, int z_index, int basis_index) {
			if(Basis::degree == 0) {
				return x_index + y_index * dofx + z_index * dofx * dofy;
			}
			int xi = x_index*(dof_per_cell1d-1) + basis_index % dof_per_cell1d;
			int yi = y_index*(dof_per_cell1d-1) + (basis_index / dof_per_cell1d)  % dof_per_cell1d ;
			int zi = z_index*(dof_per_cell1d-1) + basis_index / (dof_per_cell1d * dof_per_cell1d);
			return xi + yi * dofx + zi * dofx * dofy;
		}
		int get_dof() { return dof; }
		int get_dof_per_cell() { return dof_per_cell; }
	/*int get_n_cells() { return grid->;}
	int dof_index(int cell_index, int local_index) {
		return  cell_index*(dof_per_cell-1)+local_index;
	}*/

	};

template<class Basis, class T>
class MeshRegularNodal<1, Basis, T> {
public:
	typedef Basis basis_type;
	T x1, x2;
	int n_cells_x;
	Grid1dRegular<> xgrid;
	Grid1dRegular<>& grid;
	MatrixXd M;
//Transformation1d_in_3d* transformation;
	int dof, dofx;
	int dof1d;
	enum { dof_per_cell = MeshRegularNodalHelper<1, Basis, T>::dof_per_cell };
	enum { dof_per_cell1d = MeshRegularNodalHelper<1, Basis, T>::dof_per_cell };

	MeshRegularNodal(T x1, T x2, int n_cells_x) : n_cells_x(n_cells_x), xgrid(x1, x2, n_cells_x+1), grid(xgrid), M(1, 1) {

	if(Basis::degree == 0) {
		dofx = n_cells_x;
	} else {
		dofx = (1 + n_cells_x) + (dof_per_cell1d-2)*n_cells_x; // 1 dof per border + dofs inside the cel
	}
	dof = dofx;
	cout << "n_cells_x = " << n_cells_x << " dofx = " << dofx << endl;
	cout << "dof = " << dof << " dof_per_cell = " << dof_per_cell << endl;
	//MatrixXd m = MatrixXd::Zero(dof, dof);
	M.resize(dof, dof);
	M =  MatrixXd::Zero(dof, dof);
	//T scale = 1; //TODO: (x2-x1)/n_cells;

	//basis_integrator<1, Basis::degree, Basis::degree, Basis, Basis, Basis, T> bi;
	//cout  << "test 00 " << bi.integrate(0, 0) << endl;
	//cout  << "test 01 " << bi.integrate(0, 1) << endl;
	//cout  << "test 11 " << bi.integrate(1, 1) << endl;

	T integrals[dof_per_cell][dof_per_cell];
	for(int j = 0; j < dof_per_cell; j++) {
		for(int k = 0; k < (j+1); k++) {
			basis_integrator<2, Basis::degree, Basis::degree, Basis, Basis, Basis, T> bi;
			T integral = bi.integrate(j, k);
			//typedef selfintegrator<Basis::degree, Basis::degree> selfintegrator_type;
			//selfintegrator_type si;
			//double integral = si.integrate(j,k);
			//cout << j << " " << k << " " << integral << endl;
			integrals[j][k] = integral;
			integrals[k][j] = integral;
		}
	}
	for(int xi = 0; xi < n_cells_x; xi++) {
		for(int j = 0; j < dof_per_cell; j++) {
			for(int k = 0; k < dof_per_cell; k++) {
				int i1 = this->dof_index(xi, j);
				int i2 = this->dof_index(xi, k);
				//cout << "xi = " << xi << " yi = " << yi << " zi = " << zi << " j = " << j << " k = " << k;
				//cout << "            i1 = " << i1 << " i2 = " << i2 << endl;
				M(i1, i2) += integrals[j][k];
			}}
	}
	/*for(int i = 0; i < n_cells; i++) {
		for(int j = 0; j < dof_per_cell; j++) {
			for(int k = 0; k < dof_per_cell; k++) {
				int i1 = i*(dof_per_cell-1)+j;
				int i2 = i*(dof_per_cell-1)+k;
				m(i1, i2) = m(i1, i2) + integrals[j][k];
			}
		}
	}*/
	//cout << M << endl;
	//m = m * scale;
	//ctrans = m.inverse();
}

	double eval_(double_vector solution_, double x) {
		VectorXd solution = VectorXd::Map(solution_.data().begin(), solution_.size());
		return eval(solution, x);
	}

	template<class Array>
	double eval(Array& solution, double x) {
	/*int cell_index = mesh.findindex(x);
	T xleft = mesh.indexto_x(cell_index);
	T xright = mesh.indexto_x(cell_index+1);
	*/
		int xi = xgrid.findindex(x);
		T x1 = xgrid.indexto_x(xi);
		T x2 = xgrid.indexto_x(xi+1);
		double u = (x-x1)/(x2-x1);
	//grid.index
	//T dx = xright-xleft;
		double value = 0;
	//cout << "xi = " << xi;
	//cout << " yi = " << yi;
		if(grid.inrange(x)) {
			basis_evaluator<1, Basis> basis_evaluator;
			for(int i = 0; i < dof_per_cell; i++) {
			//cout << " i = " << i;
			//cout << " index = " << dof_index(xi, yi, i) << endl;
				double a = solution[dof_index(xi, i)];
				value += a * basis_evaluator.eval(i, u);
			}
		} else {
		cout << "x not in range: (" << x << endl;
		}
		return value;
	}

	double eval(VectorXd& solution, double x) {
	/*int cell_index = mesh.findindex(x);
	T xleft = mesh.indexto_x(cell_index);
	T xright = mesh.indexto_x(cell_index+1);
	*/
		int xi = xgrid.findindex(x);
		T x1 = xgrid.indexto_x(xi);
		T x2 = xgrid.indexto_x(xi+1);
		double u = (x-x1)/(x2-x1);
	//grid.index
	//T dx = xright-xleft;
		double value = 0;
	//cout << "xi = " << xi;
	//cout << " yi = " << yi;
		if(grid.inrange(x)) {
			basis_evaluator<1, Basis> basis_evaluator;
			for(int i = 0; i < dof_per_cell; i++) {
			//cout << " i = " << i;
			//cout << " index = " << dof_index(xi, yi, i) << endl;
				double a = solution(dof_index(xi, i));
				value += a * basis_evaluator.eval(i, u);
			}
		} else {
			cout << "x not in range: (" << x << endl;
		}
		return value;
	}

	double basis_uv(int i, double u) {
		basis_evaluator<1, Basis> basis_evaluator;
		return basis_evaluator.eval(i, u);
	}

	void solve_coordinates(double_vector inner_products, double_vector coordinates) {
		assert((int)inner_products.size() == dof);
		assert((int)coordinates.size() == dof);
		VectorXd x = VectorXd::Map(inner_products.data().begin(), inner_products.size());
		VectorXd a(dof);
		M.llt().solve(x, &a);
		VectorXd::Map(coordinates.data().begin(), coordinates.size()) = a;
	}

	/*void test(double_vector result_, double scale1, double scale2) {
		auto f = [&](double x, double y) -> double { return cos(x * scale1 + y * scale2); };
		VectorXd x = VectorXd::Zero(dof);
		assert(result_.size() == dof);
		for(int yi = 0; yi < n_cells_y; yi++) {
			for(int xi = 0; xi < n_cells_x; xi++) {
				for(int i = 0; i < dof_per_cell; i++) {
					double x1, x2;
					double y1, y2;
					grid.indexto_xy(xi, yi, x1, y1);
					grid.indexto_xy(xi+1, yi+1, x2, y2);
				//cout << "integrate x[" << x1 << " " << x2 << "] y[" << y1 << " " << y2 << "]";
					basis_function_integrator<2, Basis::degree, Basis, Basis, T> bfi(x1, x2, y1, y2);
					double a = bfi.integrate_function2d(f, i);
					x(dof_index(xi, yi, i)) += a;
				//cout << " " << a;
				//cout << endl;
				}
			}
		}
		VectorXd a(dof);
		M.llt().solve(x, &a);
	//VectorXd result = VectorXd::Map(result_.data().begin(), result_.size());
		VectorXd::Map(result_.data().begin(), result_.size()) = a;
	//result = a;
	}*/

//int get_dof() { return dof; }
	int dof_index(int x_index, int basis_index) {
		if(Basis::degree == 0) {
			return x_index;
		}
		int xi = x_index*(dof_per_cell1d-1) + basis_index % dof_per_cell1d;
		return xi;
	}
	int get_dof() { return dof; }
	int get_dof_per_cell() { return dof_per_cell; }
/*int get_n_cells() { return grid->;}
int dof_index(int cell_index, int local_index) {
	return  cell_index*(dof_per_cell-1)+local_index;
}*/

};

}


//
// Created by h on 20/07/18.
//

#include "species.h"

#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/bessel_prime.hpp>

Species::Species(const boost::python::object &species, double B) {
    charge = p::extract<double>(species.attr("charge"));
    mass = p::extract<double>(species.attr("mass"));
    density = p::extract<double>(species.attr("density"));
    wc = B * abs(charge) / mass;
    wp = pow(density * charge * charge / (e0 * mass), 0.5);

    vpara_h = arrayutils::extract1d(species.attr("vpara_h"));
    vperp_h = arrayutils::extract1d(species.attr("vperp_h"));

    npara = vpara_h.size();
    nperp = vperp_h.size();

    df_dvpara_h = arrayutils::extract2d(species.attr("df_dvpara_h"), npara, nperp);
    df_dvperp_h = arrayutils::extract2d(species.attr("df_dvperp_h"), npara, nperp);

    p::list pyns = p::extract<p::list>(species.attr("ns"));
    for (int i = 0; i < len(pyns); ++i) {
        int n{p::extract<int>(pyns[i])};
        ns.push_back(n);
        jns.push_back(arr1d(nperp));
        jnps.push_back(arr1d(nperp));
        qs.push_back({arr1d(npara), arr1d(npara), arr1d(npara), arr1d(npara), arr1d(npara), arr1d(npara)});
        q_ms.push_back({arr1d(npara - 1), arr1d(npara - 1), arr1d(npara - 1), arr1d(npara - 1), arr1d(npara - 1),
                        arr1d(npara - 1)});
        q_cs.push_back({arr1d(npara - 1), arr1d(npara - 1), arr1d(npara - 1), arr1d(npara - 1), arr1d(npara - 1),
                        arr1d(npara - 1)});
    }

    dvperp_h = arr1d(nperp - 1);
    vpara_hp = arr1d(npara - 1);
    vpara_hm = arr1d(npara - 1);
    dv1 = arr1d(npara - 1);
    dv2 = arr1d(npara - 1);
    dv3 = arr1d(npara - 1);
    dv4 = arr1d(npara - 1);
    dv5 = arr1d(npara - 1);
    dv6 = arr1d(npara - 1);
    dv7 = arr1d(npara - 1);
    dv8 = arr1d(npara - 1);

    for (size_t i = 0; i < nperp - 1; i++) {
        dvperp_h[i] = vperp_h[i + 1] - vperp_h[i];
    }

    for (size_t i = 0; i < npara - 1; i++) {
        vpara_hm[i] = vpara_h[i];
        vpara_hp[i] = vpara_h[i + 1];
        dv1[i] = pow(vpara_hp[i], 1) - pow(vpara_hm[i], 1);
        dv2[i] = pow(vpara_hp[i], 2) - pow(vpara_hm[i], 2);
        dv3[i] = pow(vpara_hp[i], 3) - pow(vpara_hm[i], 3);
        dv4[i] = pow(vpara_hp[i], 4) - pow(vpara_hm[i], 4);
        dv5[i] = pow(vpara_hp[i], 5) - pow(vpara_hm[i], 5);
        dv6[i] = pow(vpara_hp[i], 6) - pow(vpara_hm[i], 6);
        dv7[i] = pow(vpara_hp[i], 7) - pow(vpara_hm[i], 7);
        dv8[i] = pow(vpara_hp[i], 8) - pow(vpara_hm[i], 8);
    }

}

void Species::push_kperp(const double kperp) {
    for (size_t ni = 0; ni < ns.size(); ni++) {
        int n{ns[ni]};
        double jns0 = bm::cyl_bessel_j(n, -(kperp / wc) * vperp_h[0]);
        double jnps0 = bm::cyl_bessel_j(n, -(kperp / wc) * vperp_h[0]);
        for (size_t j = 0; j < nperp; j++) {
            jns[ni][j] = bm::cyl_bessel_j(n, (kperp / wc) * vperp_h[j]);
            jnps[ni][j] = bm::cyl_bessel_j_prime(n, (kperp / wc) * vperp_h[j]);
        }
        for (size_t i = 0; i < npara; i++) {
            double qh[6]{};
            qh[0] = 0.5 * jns0 * jns0 * df_dvperp_h[i][0] * dvperp_h[0];
            qh[1] = 0.5 * jns0 * jns0 * vperp_h[0] * df_dvpara_h[i][0] * dvperp_h[0];
            qh[2] = 0.5 * jns0 * jnps0 * vperp_h[0] * df_dvperp_h[i][0] * dvperp_h[0];
            qh[3] = 0.5 * jns0 * jnps0 * vperp_h[0] * vperp_h[0] * df_dvpara_h[i][0] * dvperp_h[0];
            qh[4] = 0.5 * jnps0 * jnps0 * vperp_h[0] * vperp_h[0] * df_dvperp_h[i][0] * dvperp_h[0];
            qh[5] = 0.5 * jnps0 * jnps0 * vperp_h[0] * vperp_h[0] * vperp_h[0] * df_dvpara_h[i][0] * dvperp_h[0];
            for (size_t j = 0; j < nperp - 1; j++) {
                qh[0] += jns[ni][j] * jns[ni][j] * df_dvperp_h[i][j] * dvperp_h[j];
                qh[1] += jns[ni][j] * jns[ni][j] * vperp_h[j] * df_dvpara_h[i][j] * dvperp_h[j];
                qh[2] += jns[ni][j] * jnps[ni][j] * vperp_h[j] * df_dvperp_h[i][j] * dvperp_h[j];
                qh[3] += jns[ni][j] * jnps[ni][j] * vperp_h[j] * vperp_h[j] * df_dvpara_h[i][j] * dvperp_h[j];
                qh[4] += jnps[ni][j] * jnps[ni][j] * vperp_h[j] * vperp_h[j] * df_dvperp_h[i][j] * dvperp_h[j];
                qh[5] += jnps[ni][j] * jnps[ni][j] * vperp_h[j] * vperp_h[j] * vperp_h[j] * df_dvpara_h[i][j] *
                         dvperp_h[j];
            }
            for (size_t k = 0; k < 6; k++) {
                qs[ni][k][i] = qh[k];
            }
        }
        for (size_t i = 0; i < npara - 1; i++) {
            for (size_t k = 0; k < 6; k++) {
                q_ms[ni][k][i] = (qs[ni][k][i + 1] - qs[ni][k][i]) / dv1[i];
                q_cs[ni][k][i] = qs[ni][k][i] - q_ms[ni][k][i] * vpara_hm[i];
            }
        }
    }
}

array<array<cdouble, 3>, 3> Species::push_omega(const double kpara, const double kperp, const double wr, const double wi) {
    array<array<cdouble, 3>, 3> XP{};
    for (size_t ni = 0; ni < ns.size(); ni++) {
        int n{ns[ni]};
        double a{wr};
        double b{wi};
        double b2{b * b};
        double d{n * wc};
        cdouble apb{a, b};
        cdouble apbmd{a - d, b};
        cdouble apbmd2 = apbmd*apbmd;
        cdouble apbmd3 = apbmd2*apbmd;
        cdouble iapb{1.0 / apb};
        cdouble iapbmd{1.0 / apbmd};

        array<cdouble, 6> s_L0_q_m{};
        array<cdouble, 6> s_L0_q_c{};
        array<cdouble, 6> s_L1_q_m{};
        array<cdouble, 6> s_L1_q_c{};
        array<cdouble, 6> s_L2_q_m{};
        array<cdouble, 6> s_L2_q_c{};
        array<cdouble, 6> s_L3_q_m{};

        for (size_t i = 0; i < npara - 1; i++) {
            //cdouble clogfac{log((apb - d - vpara_hp[i]*kpara)/(apb - d - vpara_hm[i]*kpara))};
            //cdouble clogfac{log((apb - d - vpara_hm[i]*kpara - dv1[i]*kpara)/(apb - d - vpara_hm[i]*kpara))};
            //cdouble clogfac{log(1.0 - (dv1[i]*kpara)/(apb - d - vpara_hm[i]*kpara))};

            //Wish to taylor expand in kpara.
            //cdouble cx = (-dv1[i]*kpara)/(apbmd - vpara_hm[i]*kpara);
            cdouble cx = kpara*iapbmd;
            cdouble L0, L1, L2, L3;
            const double narr[]{1./1., 1./2., 1./3., 1./4., 1./5., 1./5., 1./7., 1./8.};
            if ((vpara_hm[i]+vpara_hp[i])*(vpara_hm[i]+vpara_hp[i])*(cx.real()*cx.real() + cx.imag()*cx.imag()) < -0.000001){
                cdouble logfacsum{0.0, 0.0};
                cdouble powarr[8];
                const cdouble dvarr[]{dv1[i], dv2[i], dv3[i], dv4[i], dv5[i], dv6[i],dv7[i], dv8[i]};
                powarr[0] = cx;
                for (size_t i = 1; i < 8;i++){
                    powarr[i] = powarr[i-1]*(cx);
                }
                for (size_t i = 8; i > 3; i--) {
                    logfacsum -= powarr[i-1]*(narr[i-1]*dvarr[i-1]);
                }
                L3 = logfacsum;
                L2 = L3 - powarr[2]*narr[2]*dvarr[2];
                L1 = L2 - powarr[1]*narr[1]*dvarr[1];
                L0 = L1 - powarr[0]*narr[0]*dvarr[0];
                if (wi<0.0){
                    cdouble di = I*L0.imag();
                    L0 -= di;
                    L1 -= di;
                    L2 -= di;
                    L3 -= di;
                }
            }else{
                double advkm = a - d - vpara_hm[i] * kpara;
                double rlogfac = 0.5 * log1p(dv1[i] * kpara * (dv1[i] * kpara - 2.0 * advkm) / (advkm * advkm + b2));
                double ilogfac = atan2(abs(b)*dv1[i]*kpara, (advkm + dv1[i]*kpara)*advkm + b2);
                L0 = cdouble(rlogfac, ilogfac);
                cdouble powarr[3];
                const cdouble dvarr[]{dv1[i], dv2[i], dv3[i]};
                powarr[0] = cx;
                for (size_t i = 1; i < 3;i++){
                    powarr[i] = powarr[i-1]*(cx);
                }
                L1 = L0 + powarr[0]*narr[0]*dvarr[0];
                L2 = L1 + powarr[1]*narr[1]*dvarr[1];
                L3 = L2 + powarr[2]*narr[2]*dvarr[2];
            }

            for (size_t k = 0; k < 6; k++) {
                s_L0_q_m[k] += L0 * q_ms[ni][k][i];
                s_L0_q_c[k] += L0 * q_cs[ni][k][i];
                s_L1_q_m[k] += L1 * q_ms[ni][k][i];
            }

            for (size_t k = 0; k < 5; k++) {
                s_L1_q_c[k] += L1 * q_cs[ni][k][i];
                s_L2_q_m[k] += L2 * q_ms[ni][k][i];
            }

            s_L2_q_c[0] += L2 * q_cs[ni][0][i];
            s_L2_q_c[2] += L2 * q_cs[ni][2][i];
            s_L3_q_m[0] += L3 * q_ms[ni][0][i];
            s_L3_q_m[2] += L3 * q_ms[ni][2][i];
        }

        double kpara2 = kpara*kpara;
        double kpara3 = kpara2*kpara;
        double kpara4 = kpara3*kpara;
        double kperp2 = kperp*kperp;

        cdouble c00 = M_PI * 2.0 * wp * wp * wc * wc * n * n * iapb * iapb;
        XP[0][0] -= c00*apb*(kpara2*apbmd*s_L1_q_m[0] + kpara3*s_L0_q_c[0]);
        XP[0][0] -= c00*(kpara3*apbmd*s_L1_q_m[1] + kpara4*s_L0_q_c[1]);
        XP[0][0] += c00*(kpara2*apbmd2*s_L2_q_m[0] + kpara3*apbmd*s_L1_q_c[0]);

        cdouble c01 = M_PI * 2.0 * wp * wp * n * wc * I * iapb * iapb * kperp;
        XP[0][1] -= c01*apb*(kpara2*apbmd*s_L1_q_m[2] + kpara3*s_L0_q_c[2]);
        XP[0][1] -= c01*(kpara3*apbmd*s_L1_q_m[3] + kpara4*s_L0_q_c[3]);
        XP[0][1] += c01*(kpara2*apbmd2*s_L2_q_m[2] + kpara3*apbmd*s_L1_q_c[2]);

        cdouble c02 = M_PI * 2.0 * wp * wp * n * wc * iapb * iapb * kperp;
        XP[0][2] -= c02*apb*(kpara*apbmd2*s_L2_q_m[0] + kpara2*apbmd*s_L1_q_c[0]);
        XP[0][2] -= c02*(kpara2*apbmd2*s_L2_q_m[1] + kpara3*apbmd*s_L1_q_c[1]);
        XP[0][2] += c02*(kpara*apbmd3*s_L3_q_m[0] + kpara2*apbmd2*s_L2_q_c[0]);

        cdouble c11 = M_PI * 2.0 * wp * wp * iapb * iapb * kperp2;
        XP[1][1] -= c11*apb*(kpara2*apbmd*s_L1_q_m[4] + kpara3*s_L0_q_c[4]);
        XP[1][1] -= c11*(kpara3*apbmd*s_L1_q_m[5] + kpara4*s_L0_q_c[5]);
        XP[1][1] += c11*(kpara2*apbmd2*s_L2_q_m[4] + kpara3*apbmd*s_L1_q_c[4]);

        cdouble c12 = M_PI * 2.0 * wp * wp * -I * iapb * iapb * kperp2;
        XP[1][2] -= c12*apb*(kpara*apbmd2*s_L2_q_m[2] + kpara2*apbmd*s_L1_q_c[2]);
        XP[1][2] -= c12*(kpara2*apbmd2*s_L2_q_m[3] + kpara3*apbmd*s_L1_q_c[3]);
        XP[1][2] += c12*(kpara*apbmd3*s_L3_q_m[2] + kpara2*apbmd2*s_L2_q_c[2]);

        cdouble c22 = M_PI * 2.0 * wp * wp * iapb * iapb * kperp2;
        XP[2][2] -= c22*apbmd*(kpara*apbmd2*s_L2_q_m[1] + kpara2*apbmd*s_L1_q_c[1]);
        XP[2][2] -= c22*d*(apbmd3*s_L2_q_m[0] + kpara*apbmd2*s_L2_q_c[0]);
    }

    XP[1][0] = -XP[0][1];
    XP[2][0] = XP[0][2];
    XP[2][1] = -XP[1][2];

    return XP;
}


#include <Engine/MeshEdit/Simulate_fast.h>

#include <math.h>
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/Dense>

using namespace Ubpa;

using namespace std;
using namespace Eigen;

void Simulate::Clear() {
	this->positions.clear();
}

bool Simulate::Init() {
	m = positions.size() / 3; // number of vertices
	s = edgelist.size() / 2;  // number of springs
	g = 9.8;
	h = 1 / 30;
	iteration = 10;

	// init q_n-1, q_n, q_n+1
	x.resize(3 * m); // vector x, initialized to be y
	y.resize(3 * m); // vector y = 2q_n - q_n-1
	for (int i = 0; i < m; i++) {
		x.segment(3 * i, 3) << positions[i][0]
			<< positions[i][1]
			<< positions[i][2];
		y.segment(3 * i, 3) = x.segment(3 * i, 3);
	}

	// init Mass
	M.resize(3 * m);
	M.setOnes();

	// init force_ext, i.e. gravity
	f_ext.resize(3 * m);
	for (int i = 0; i < m; i++)
		f_ext.segment(3 * i, 3) = Vector3d(0, 0, -1);

	L = MatrixXd::Zero(m * 3, m * 3);
	buildL();
	J = MatrixXd::Zero(m * 3, m * 3);
	buildJ();

	FixPoint();
	buildK();
	getb();

	// prefactorization
	MatrixXd A_ = K * (M + h * h * L) * K.transpose() * K;
	A = A_.sparseView();
	LLT_.compute(A);

	return true;
}

void Simulate::SetLeftFix()
{
	//固定网格x坐标最小点
	fixed_id.clear();
	double x = 100000;
	for (int i = 0; i < positions.size(); i++)
	{
		if (positions[i][0] < x)
		{
			x = positions[i][0];
		}
	}

	for (int i = 0; i < positions.size(); i++)
	{
		if (abs(positions[i][0] - x) < 1e-5)
		{
			fixed_id.push_back(i);
		}
	}

	Init();
}

void Simulate::FixPoint() {
	fixed_id.push_back(0);
	fixed_id.push_back(m / 4);
}

void Simulate::buildK() {
	K = MatrixXd::Ones(m * 3 - 3 * fixed_id.size(), m * 3);

	for (int i = 0, j = 0; i < xk.size(); i++) {
		if (fix.find(i) == fix.end()) {
			K(j++, i) = 1;
		}
	}

	// Eigen::MatrixXd xt;
	// xt.resize((x.size()),1);
	// b.resize(x.size());

	// for (int i = 0; i < x.size(); i++)
	// 	xt(i, 0) = x[i];
	// Eigen::MatrixXd t = K.transpose() * K * xt;

	// for (int i = 0; i < xk.size(); i++)
	// 	b[i] = x[i] - t(i, 0);
}

void Simulate::buildL() {
	size_t m = positions.size() / 3;
	size_t s = edgelist.size() / 2;

	MatrixXd temp = MatrixXd::Zero(m, m);

	for (size_t i = 0; i < s; i++) {
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];

		VectorXd Ai = VectorXd::Zero(m);
		Ai(index1 - 1) += 1;
		Ai(index2 - 1) -= 1;
		temp += stiff * Ai * Ai.transpose();
	}

	Matrix3d I3 = Matrix3d::Identity();

	// kronecker product, L = kronecker(temp, I3)
	for (int i = 0; i < m; i++) {
		L.block(i * 3, i * 3, 3, 3) = temp(i, i) * I3;
	}
}

void Simulate::buildJ() {
	MatrixXd temp = MatrixXd::Zero(m, s);

	for (size_t i = 0; i < s; i++) {
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];

		VectorXd Ai = VectorXd::Zero(m);
		VectorXd Si = VectorXd::Zero(s);
		Ai(index1 - 1) += 1;
		Ai(index2 - 1) -= 1;
		Si(i) += 1;
		temp += stiff * Ai * Si.transpose();
	}

	Matrix3d I3 = Matrix3d::Identity();

	// kronecker product, L = kronecker(temp, I3)
	for (int i = 0; i < m; i++) {
		L.block(i * 3, i * 3, 3, 3) = temp(i, i) * I3;
	}

}

void Simulate::local() {
	d = VectorXd::Ones(s * 3);

	for (size_t i = 0; i < s; i++) {
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];
		pointf3 p1 = positions[index1];
		pointf3 p2 = positions[index2];
		vecf3 r = p1 - p2;

		VectorXd di(3);
		di << r[0], r[1], r[2];

		d.segment(3 * i, 3) = l[i] * di / pointf3::distance(p1, p2);
	}

}

void Simulate::global() {
	VectorXd RHS = K * (h * h * J * d + M * y - h * h * f_ext - (M + h * h * L) * b);

	VectorXd xf = LLT_.solve(RHS);

	x = K.transpose() * xf + b;

}

void Simulate::getb() {
	b = x - K.transpose() * K * x;
}

void Simulate::UpdatePos() {
	for (int i = 0; i < m * 3; i++)
		positions[i / 3][i % 3] = x(i);
}

void Simulate::SimulateOnce() {
	//update y, y = 2q_n - q_n-1
	y = 2 * x - y;
	size_t step = 0;
	while (step < iteration) {
		local();
		global();
	}
	UpdatePos();
}

bool Simulate::Run() {
	SimulateOnce();
	return true;
}


// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Paul Ferrand

#include "helpers.h"
#define EIGEN_MPL2_ONLY
#include <Eigen/Dense>
#include <numeric>

using namespace std::complex_literals;

template<class T>
constexpr T pi = T { 3.14159265358979323846 };

std::pair<float, std::complex<float>> frequencyPeakSearch(float* signal, size_t size, float coarseFrequency,
    float sampleRate, float range, int resolution)
{
    using namespace Eigen;

    // Build the super-resolution time-frequency matrix around the coarse frequency
    VectorXf freq = VectorXf::LinSpaced(2 * resolution + 1, coarseFrequency - range, coarseFrequency + range);
    VectorXf time = VectorXf::LinSpaced(size, 0, static_cast<float>(size - 1)) / sampleRate;

    MatrixXcf projectionMatrix =
        exp(2.0if * pi<float> * (freq * time.transpose()).array());

    // Project the signal on the matrix
    VectorXcf projected = projectionMatrix * Map<VectorXf>(signal, size);

    // Find the highest harmonic
    unsigned maxIdx = 0;
    float maxHarmonic = 0.0f;
    for (unsigned i = 0; i < projected.size(); ++i) {
        float harmonic = std::abs(projected[i]);
        if (harmonic > maxHarmonic) {
            maxIdx = i;
            maxHarmonic = harmonic;
        }
    }

    return std::make_pair(freq[maxIdx], projected[maxIdx]);
}

std::vector<float> buildWavetable(const HarmonicVector& harmonics, int size, bool normalizePower)
{
    using namespace Eigen;

    std::vector<double> table;
    std::vector<float> output;
    table.resize(size);
    output.reserve(size);
    std::fill(table.begin(), table.end(), 0.0);

    if (harmonics.empty()) {
        std::fill_n(std::back_inserter(output), size, 0.0f);
        return output;
    }

    using RowArrayXd = Array<double, 1, Dynamic>;
    ArrayXd time = ArrayXd::LinSpaced(size, 0, static_cast<double>(size - 1));
    time /= static_cast<double>(size);
    Map<ArrayXd> mappedTable { table.data(), size };

    for (const auto& [f, h] : harmonics) {
        double freqIndex = std::round(f / harmonics.front().first);
        double phase = std::arg(h);
        double magnitude = std::abs(h);
        // fmt::print("Harmonic at {:.2f} ({}) Hz: {:.3f} exp (i pi {:.3f})\n", f, freqIndex, magnitude, phase);
        mappedTable += magnitude * (2.0 * pi<double> * freqIndex * time + phase).sin();
    }

    // Normalize the overall power
    if (normalizePower) {
        double squaredNorm = std::accumulate(harmonics.begin(), harmonics.end(), 0.0,
            [] (double lhs, const auto& rhs) { return lhs + std::pow(std::abs(rhs.second), 2); });
        double norm = std::sqrt(squaredNorm);
        mappedTable /= norm;
    }

    // Roll the wavetable to start around 0
    size_t zeroIndex = 0;
    double zeroValue = mappedTable.maxCoeff();
    for (int i = 0; i < size; ++i) {
        double absValue = std::abs(mappedTable[i]);
        if (absValue < zeroValue) {
            zeroIndex = i;
            zeroValue = absValue;
        }
    }
    ArrayXd head = mappedTable.head(zeroIndex);
    ArrayXd tail = mappedTable.tail(size - zeroIndex);
    mappedTable << tail, head;

    std::transform(table.begin(), table.end(), std::back_inserter(output),
            [](double x) { return static_cast<float>(x); });

    return output;
}

std::vector<float> extractSignalRange(const float* source, double regionStart, double regionEnd,
    double samplePeriod, int stride, int offset)
{
    std::vector<float> signal;

    if (regionStart > regionEnd)
        std::swap(regionStart, regionEnd);

    int rangeStart = static_cast<int>(regionStart / samplePeriod);
    int rangeEnd = static_cast<int>(regionEnd / samplePeriod);
    int rangeSize = rangeEnd - rangeStart;
    if (rangeSize == 0)
        return signal;

    signal.resize(rangeSize);
    for (int t = 0, s = rangeStart; s < rangeEnd; ++t, ++s)
        signal[t] = source[stride * s + offset];

    return signal;
}


#include <Eigen/Cholesky>
/*
 * Member function definitions of Kalman template class.
 * See kalman.h for template class declaration.
 */

namespace observer {

template <typename T>
Kalman<T>::Kalman(T& system) : Observer<T>(system, state_t::Zero()) {
    reset();
}

template <typename T>
Kalman<T>::Kalman(T& system, const state_t& x0) : Observer<T>(system, x0) {
    reset();
}

template <typename T>
Kalman<T>::Kalman(T& system, const state_t& x0,
        const process_noise_covariance_t& Q,
        const measurement_noise_covariance_t& R,
        const error_covariance_t& P0) : Observer<T>(system, x0),
            m_P(P0), m_Q(Q), m_R(R) {
    m_K.setZero();
}

template <typename T>
void Kalman<T>::reset() {
    m_x.setZero();
    m_P.setIdentity();
    m_Q.setIdentity();
    m_R.setIdentity();
    m_K.setZero();
}

template <typename T>
void Kalman<T>::update_state(const input_t& u, const measurement_t& z) {
    time_update(u);
    measurement_update(z);
}

template <typename T>
void Kalman<T>::time_update() {
    time_update_state();
    time_update_error_covariance();
}

template <typename T>
void Kalman<T>::time_update(const process_noise_covariance_t& Q) {
    time_update_state();
    time_update_error_covariance(Q);
}

template <typename T>
void Kalman<T>::time_update(const input_t& u) {
    time_update_state(u);
    time_update_error_covariance();
}

template <typename T>
void Kalman<T>::time_update(const input_t& u, const process_noise_covariance_t& Q) {
    time_update_state(u);
    time_update_error_covariance(Q);
}

template <typename T>
void Kalman<T>::measurement_update(const measurement_t& z) {
    measurement_update_kalman_gain();
    measurement_update_state(z);
    measurement_update_error_covariance();
}

template <typename T>
void Kalman<T>::measurement_update(const measurement_t& z, const measurement_noise_covariance_t& R) {
    measurement_update_kalman_gain(R);
    measurement_update_state(z);
    measurement_update_error_covariance();
}

template <typename T>
void Kalman<T>::time_update_state() {
    m_x = m_system.normalize_state(m_system.Ad()*m_x);
}

template <typename T>
void Kalman<T>::time_update_state(const input_t& u) {
    m_x = m_system.normalize_state(m_system.Ad()*m_x + m_system.Bd()*u);
}

template <typename T>
void Kalman<T>::time_update_error_covariance() {
    m_P = m_system.Ad()*m_P*m_system.Ad().transpose() + m_Q;
}

template <typename T>
void Kalman<T>::time_update_error_covariance(const process_noise_covariance_t& Q) {
    m_P = m_system.Ad()*m_P*m_system.Ad().transpose() + Q;
}

template <typename T>
void Kalman<T>::measurement_update_kalman_gain() {
    // S = C*P*C' + R
    // K = P*C'*S^-1 - > K' = S^-1*C*P'
    Eigen::LDLT<measurement_noise_covariance_t> S_ldlt(
            m_system.Cd()*m_P*m_system.Cd().transpose() + m_R);
    m_K.noalias() = S_ldlt.solve(m_system.Cd()*m_P.transpose()).transpose();
}

template <typename T>
void Kalman<T>::measurement_update_kalman_gain(const measurement_noise_covariance_t& R) {
    // S = C*P*C' + R
    // K = P*C'*S^-1 - > K' = S^-1*C*P'
    Eigen::LDLT<measurement_noise_covariance_t> S_ldlt(
            m_system.Cd()*m_P*m_system.Cd().transpose() + R);
    m_K.noalias() = S_ldlt.solve(m_system.Cd()*m_P.transpose()).transpose();
}

template <typename T>
void Kalman<T>::measurement_update_state(const measurement_t& z) {
    m_x = m_system.normalize_state(m_x + m_K*(
                m_system.normalize_output(z - m_system.Cd()*m_x)));
}

template <typename T>
void Kalman<T>::measurement_update_error_covariance() {
    m_P = (error_covariance_t::Identity() - m_K*m_system.Cd())*m_P;
}

} // namespace observer


#ifndef MATH_QUAT_HPP
#define MATH_QUAT_HPP

#include <boost/operators.hpp>

namespace Math {

	template<typename R = float>
	struct quat: public boost::operators<quat<R>> {
		R w, x, y, z;

		/* Unary operators */

		/** Multiplicative inverse */
		quat<R> operator!(void) const;
		/** Additive inverse */
		quat<R> operator-(void) const;
		/** Conjugate */
		quat<R> operator~(void) const;
		/** Cast operator, as Euclidean norm */
		explicit operator R(void) const;
		/** Squared Euclidean norm */
		R operator()(void) const;
		/** Simple promotion */
		explicit operator dual<R>(void) const;

		/** Distributes equality test */
		bool operator==(quat<R> const& rhs) const;
		/** Apply (lhs * rhs * ~lhs) */
		quat<R> operator()(quat<R> const& rhs) const;
		/** Apply (lhs * rhs * ~lhs) */
		dual<R> operator()(dual<R> const& rhs) const;

		/*quat<R> operator+(quat<R> const& rhs) const;
		quat<R> operator-(quat<R> const& rhs) const;
		quat<R> operator/(quat<R> const& rhs) const;
		quat<R> operator/(R const& rhs) const;*/

		quat<R>& operator+=(quat<R> const& rhs);
		quat<R>& operator-=(quat<R> const& rhs);
		quat<R>& operator*=(R const& rhs);
		quat<R>& operator*=(quat<R> const& rhs);
		quat<R>& operator/=(R const& rhs);
		quat<R>& operator/=(quat<R> const& rhs);

		quat(void) = default;
		quat(const R w, const R x = 0, const R y = 0, const R z = 0):
			w(w), x(x), y(y), z(z) {}
	};
	template<typename R>
	quat<R> quat<R>::operator-(void) const {
		return {-w,-x,-y,-z};
	}
	template<typename R>
	quat<R> quat<R>::operator!(void) const {
		return ~(*this)/((*this)());
	}
	template<typename R>
	quat<R> quat<R>::operator~(void) const {
		return {w,-x,-y,-z};
	}
	template<typename R>
	quat<R>::operator R(void) const {
		return sqrt((*this)());
	}
	template<typename R>
	R quat<R>::operator()(void) const {
		return w*w + x*x + y*y + z*z;
	}
	template<typename R>
	quat<R>::operator dual<R>(void) const {
		return {*this, 0};
	}

	template<typename R>
	bool quat<R>::operator==(quat<R> const& rhs) const {
		return w == rhs.w && x == rhs.x
			&& y == rhs.y && z == rhs.z;
	}
	template<typename R>
	quat<R> quat<R>::operator()(quat<R> const& rhs) const {
		return *this * rhs * ~*this;
	}

	template<typename R>
	quat<R>& quat<R>::operator+=(quat<R> const& rhs) {
		w += rhs.w; x += rhs.x; y += rhs.y; z += rhs.z;
		return *this;
	}
	template<typename R>
	quat<R>& quat<R>::operator-=(quat<R> const& rhs) {
		w -= rhs.w; x -= rhs.x; y -= rhs.y; z -= rhs.z;
		return *this;
	}
	template<typename R>
	quat<R>& quat<R>::operator*=(R const& rhs) {
		w *= rhs; x *= rhs;
		y *= rhs; z *= rhs;
		return *this;
	}
	template<typename R>
	quat<R>& quat<R>::operator*=(quat<R> const& r) {
		R lw = w, lx = x, ly = y, lz = z,
		  rw = r.w, rx = r.x, ry = r.y, rz = r.z;
		w = lw*rw - lx*rx - ly*ry - lz*rz;
		x = lw*rx + lx*rw + ly*rz - lz*ry;
		y = lw*ry - lx*rz + ly*rw + lz*rx;
		z = lw*rz + lx*ry - ly*rx + lz*rw;
		return *this;
	}
}

#endif


/* $Id: RescalePseudoLog.cpp,v 1.3 2013/04/08 07:36:16 severin Exp $ */

/***

NAME - EEDB::SPStreams::RescalePseudoLog

SYNOPSIS

DESCRIPTION

  A simple signal procesor which rescale expression level as pseudo log
Log transformation is convenient to vizualize data whose expression levels
varies in a wide range of values, but zero values are common places and
log(base,0) is not defined and we would thus need to recurse to pseudocount
(typically arbitrarily adding 0.5).

Alternatively we can use pseudolog defined as asinh(x/2) / log(base), which
has the following nice properties
   * is defined for all real x values
   * pseudolog(base, 0) = 0
   * pseudolog(base, -x) = -1* pseudolog(base, x)
   * pseudolog(base, x) ~ log(base, x) for x > base values
           [ For information :                                         ]
           [       pseudolog(10,1)  = 0.2089876; log10(1)  = 0         ]
           [       pseudolog(10,2)  = 0.3827757; log10(2)  = 0.3010300 ]
           [       pseudolog(10,3)  = 0.5188791; log10(3)  = 0.4771213 ]
           [       pseudolog(10,4)  = 0.6269629; log10(4)  = 0.6020600 ]
           [       pseudolog(10,5)  = 0.7153834; log10(5)  = 0.6989700 ]
           [       pseudolog(10,10) = 1.0042792; log10(10) = 1         ]
           [       pseudolog(10,100)= 2.0000430; log10(100)= 2         ]
           [       pseudolog(2,1)  = 0.6942419; log2(1)  = 0           ]
           [       pseudolog(2,2)  = 1.2715533; log2(2)  = 1           ]
           [       pseudolog(2,3)  = 1.7236790; log2(3)  = 1.584963    ]
           [       pseudolog(2,4)  = 2.0827257; log2(4)  = 2           ]
           [       pseudolog(2,5)  = 2.3764522; log2(5)  = 2.321928    ]
           [       pseudolog(2,10) = 3.3361433; log2(10) = 3.321928    ]
           [       pseudolog(2,100)= 6.6440004; log2(100)= 6.643856    ]

CONTACT

Nicolas Bertin <nbertin@gsc.riken.jp>
Jessica Severin <severin@gsc.riken.jp>

LICENSE

 * Software License Agreement (BSD License)
 * EdgeExpressDB [eeDB] system
 * copyright (c) 2007-2013 Jessica Severin RIKEN OSC
 * All rights reserved.
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of Jessica Severin RIKEN OSC nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

APPENDIX

The rest of the documentation details each of the object methods. Internal methods are usually preceded with a _

***/

#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <string>
#include <stdarg.h>
#include <rapidxml.hpp>  //rapidxml must be include before boost
#include <boost/algorithm/string.hpp>
#include <EEDB/Feature.h>
#include <EEDB/Expression.h>
#include <EEDB/Experiment.h>
#include <EEDB/SPStream.h>
#include <EEDB/SPStreams/RescalePseudoLog.h>

using namespace std;
using namespace MQDB;

const char*  EEDB::SPStreams::RescalePseudoLog::class_name = "EEDB::SPStreams::RescalePseudoLog";

//function prototypes
void _spstream_rescalepseudolog_delete_func(MQDB::DBObject *obj) {
  delete (EEDB::SPStreams::RescalePseudoLog*)obj;
}
MQDB::DBObject* _spstream_rescalepseudolog_next_in_stream_func(EEDB::SPStream* node) {
  return ((EEDB::SPStreams::RescalePseudoLog*)node)->_next_in_stream();
}
void _spstream_rescalepseudolog_xml_func(MQDB::DBObject *obj, string &xml_buffer) {
  ((EEDB::SPStreams::RescalePseudoLog*)obj)->_xml(xml_buffer);
}
string _spstream_rescalepseudolog_display_desc_func(MQDB::DBObject *obj) {
  return ((EEDB::SPStreams::RescalePseudoLog*)obj)->_display_desc();
}

EEDB::SPStreams::RescalePseudoLog::RescalePseudoLog() {
  init();
}

EEDB::SPStreams::RescalePseudoLog::~RescalePseudoLog() {
}

void EEDB::SPStreams::RescalePseudoLog::init() {
  EEDB::SPStream::init();
  _classname                 = EEDB::SPStreams::RescalePseudoLog::class_name;
  _module_name               = "RescalePseudoLog";
  _funcptr_delete            = _spstream_rescalepseudolog_delete_func;
  _funcptr_xml               = _spstream_rescalepseudolog_xml_func;
  _funcptr_simple_xml        = _spstream_rescalepseudolog_xml_func;
  _funcptr_display_desc      = _spstream_rescalepseudolog_display_desc_func;

  //function pointer code
  _funcptr_next_in_stream         = _spstream_rescalepseudolog_next_in_stream_func;

  //attribute variables
  // default to base 10 (aka pseudolog10)
  base(10);
}

void EEDB::SPStreams::RescalePseudoLog::base(long value) {
  _base = value;
  char buffer[17];
  snprintf(buffer, 16, "_pseudolog%ld", value);
  _base_str = buffer;
}

void EEDB::SPStreams::RescalePseudoLog::base(char* value) {
  if(value==NULL) { return; }
  _base_str = string("_pseudolog") + value;
  _base = strtol(value, NULL, 10);
}

////////////////////////////////////////////////////////////////////////////
//
//  creation from XML section
//
////////////////////////////////////////////////////////////////////////////

EEDB::SPStreams::RescalePseudoLog::RescalePseudoLog(void *xml_node) {
  //constructor using a rapidxml <spstream> description
  init();
  if(xml_node==NULL) { return; }

  rapidxml::xml_node<>      *root_node = (rapidxml::xml_node<>*)xml_node;
  rapidxml::xml_node<>      *node;

  if(string(root_node->name()) != "spstream") { return; }

  if((node = root_node->first_node("base")) != NULL) {
    //base(strtol(node->value(), NULL, 10));
    base(node->value());
  }
}

string EEDB::SPStreams::RescalePseudoLog::_display_desc() {
  char buffer[256];
  snprintf(buffer, 256, "RescalePseudoLog%ld", _base);
  return buffer;
}

void EEDB::SPStreams::RescalePseudoLog::_xml(string &xml_buffer) {
  _xml_start(xml_buffer);  //from SPStream superclass

  char buffer[256];
  snprintf(buffer, 256, "<base>%ld</base>", _base);
  xml_buffer.append(buffer);

  _xml_end(xml_buffer);  //from superclass
}

////////////////////////////////////////////////////////////////////////////
//
// callback methods
//
////////////////////////////////////////////////////////////////////////////

MQDB::DBObject* EEDB::SPStreams::RescalePseudoLog::_next_in_stream() {
  if(_source_stream == NULL) { return NULL; }

  MQDB::DBObject *obj = _source_stream->next_in_stream();
  if(obj == NULL) { return NULL; }

  if(obj->classname() == EEDB::Expression::class_name) {
    EEDB::Expression *express = (EEDB::Expression*)obj;
    _process_expression(express);
  }

  else if(obj->classname() == EEDB::Feature::class_name) {
    EEDB::Feature *feature = (EEDB::Feature*)obj;
    vector<EEDB::Expression*>  expression = feature->expression_array();
    for(unsigned int i=0; i<expression.size(); i++) {
      _process_expression(expression[i]);
    }
    feature->rebuild_expression_hash();
  }
  //other classes are not modified

  //everything is just passed through
  return obj;
}

void  EEDB::SPStreams::RescalePseudoLog::_process_expression(EEDB::Expression *express) {
  if(express == NULL) { return; }
  EEDB::Experiment *exp = express->experiment();
  if(exp == NULL) { return; }
  EEDB::Datatype *dtype = express->datatype();
  if(dtype == NULL) { return; }

  double tval =  asinh( express->value() / 2) / log(_base) ;
  express->value(tval);
  express->datatype(dtype->type() + _base_str);
}


#include <iostream>
#include <fstream>
#include <cstdio>
#include <complex>
#include <set>
#include <cmath>
#include <map>
#include <ctime>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <gflags/gflags.h>
#include <redsvd/redsvd.hpp>
#include <boost/math/special_functions/bessel.hpp>

using namespace Eigen;
using namespace REDSVD;
using namespace boost;
using namespace std;

const float EPS = 0.00000000001f;

DEFINE_string(filename, "test.ungraph", "Filename for edgelist file.");
DEFINE_string(emb1, "sparse.emb", "Filename for svd results.");
DEFINE_string(emb2, "spectral.emb", "Filename for svd results.");
DEFINE_int32(num_node, 4, "Number of node in the graph.");
DEFINE_int32(num_rank, 2, "Embedding dimension.");
DEFINE_int32(num_step, 5, "Number of order for recursion.");
DEFINE_int32(num_iter, 2, "Number of iter in randomized svd.");
DEFINE_int32(num_thread, 10, "Number of threads.");
DEFINE_double(theta, 0.5, "Parameter of ProNE");
DEFINE_double(mu, 0.1, "Parameter of ProNE");

SMatrixXf readGraph(string filename, int num_node){
    SMatrixXf A(num_node, num_node);
    typedef Eigen::Triplet<float> T;
    vector<T> tripletList;
    ifstream fin(filename.c_str());
    while (1)
    {
        string x, y;
        if (!(fin >> x >> y))
            break;
        int a = atoi(x.c_str()), b = atoi(y.c_str());
        if (a==b) continue;
        tripletList.push_back(T(a, b, 1));
        tripletList.push_back(T(b, a, 1));
    }
    A.setFromTriplets(tripletList.begin(), tripletList.end());
    return A;
}

void saveEmbedding(MatrixXf &data, string output){
    int m = data.rows(), d = data.cols();
    FILE *emb = fopen(output.c_str(), "wb");
    fprintf(emb, "%d %d\n", m, d);
    for (int i = 0; i < m; i++)
    {
        fprintf(emb, "%d", i);
        for (int j = 0; j < d; j++)
            fprintf(emb, " %f", data(i, j));
        fprintf(emb, "\n");
    }
    fclose(emb);
}

SMatrixXf l1Normalize(SMatrixXf & mat){
    SMatrixXf mat2(mat.rows(), mat.cols());
    for (int k=0; k<mat.outerSize(); ++k){
        int num_neighbor = mat.row(k).sum();
        for (SMatrixXf::InnerIterator it(mat,k); it; ++it)
            mat2.insert(k, it.col()) = it.value()/num_neighbor;
    }
    return mat2;
}

MatrixXf & l2Normalize(MatrixXf & mat){
    for (int i = 0; i < mat.rows(); ++i){
        float ssn = sqrt(mat.row(i).squaredNorm());
        if (ssn < EPS) ssn = EPS;
        mat.row(i) = mat.row(i) / ssn;
      }
    return mat;
}

SMatrixXf & validate(SMatrixXf & mat){
    for (int k=0; k<mat.outerSize(); ++k)
          for (SMatrixXf::InnerIterator it(mat,k); it; ++it)
              if (it.value() <=0)
                mat.coeffRef(k, it.col()) = 1;
    return mat;
}

SMatrixXf & smfLog(SMatrixXf & mat){
    for (int k=0; k<mat.outerSize(); ++k)
          for (SMatrixXf::InnerIterator it(mat,k); it; ++it)
              mat.coeffRef(it.row(), it.col()) = log(it.value());
    return mat;
}

float bessel(int a, float b){
    return boost::math::cyl_bessel_i(a, b);
}

void printSmf(SMatrixXf & mat){
    for (int k=0; k<mat.outerSize(); ++k)
          for (SMatrixXf::InnerIterator it(mat,k); it; ++it)
            cout <<"(" <<k << ", "<<it.col()<<", "<<it.value()<<")"<<endl;
}

MatrixXf & svdFlip(MatrixXf & mat){
    VectorXf max_abs_num = mat.cwiseAbs().colwise().maxCoeff();
    for (int i = 0; i < mat.cols(); ++i){
        float sign = max_abs_num(i) >= 0? 1.0:-1.0;
        mat.col(i) = mat.col(i) * sign;
      }
    return mat;
}

MatrixXf randomizedRangeFinder(SMatrixXf &A, int size, int num_iter){
    int n_samples = A.rows(), n_features= A.cols();
    MatrixXf Q = MatrixXf::Random(n_features, size), L(n_samples, size);
    Eigen::FullPivLU<MatrixXf> lu1(n_samples, size);
    Eigen::FullPivLU<MatrixXf> lu2(n_features, size);
    for(int i=0; i<num_iter;i++)
    {
        lu1.compute(A * Q);
        L.setIdentity();
        L.block(0, 0, n_samples, size).triangularView<Eigen::StrictlyLower>() = lu1.matrixLU();
        L = lu1.permutationP().inverse() * L;

        lu2.compute(A.transpose() * L);
        Q.setIdentity();
        Q.block(0, 0, n_features, size).triangularView<Eigen::StrictlyLower>() = lu2.matrixLU();
        Q = lu2.permutationP().inverse() * Q;
    }
    Eigen::ColPivHouseholderQR<MatrixXf> qr(A * Q);
    // return qr.colsPermutation().inverse() * qr.householderQ();
    return qr.householderQ() * MatrixXf::Identity(n_samples, size);
}

MatrixXf randomizedSvd(SMatrixXf &data, int rank, int num_iter){
    int n_oversamples = 10;
    int n_random = rank + n_oversamples;
    int n_samples = data.rows(), n_features= data.cols();
    if(n_random > min(n_samples, n_features))
        n_random = min(n_samples, n_features);

    MatrixXf Q = randomizedRangeFinder(data, n_random, num_iter);
    cout <<"Q computed done"<<endl;
    MatrixXf B = Q.transpose() * data;

    // Eigen::JacobiSVD<MatrixXf> svdOfB(B, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::BDCSVD<Eigen::MatrixXf> svdOfB(B, Eigen::ComputeThinU);

    VectorXf s = svdOfB.singularValues();
    // MatrixXf V = svdOfB.matrixV();
    MatrixXf U = Q * svdOfB.matrixU();

    U = svdFlip(U);

    MatrixXf newU = U.block(0, 0, n_samples, rank);
    // MatrixXf V = svdOfB.matrixV().block(0, 0, n_samples, rank);
    VectorXf newS = s.head(rank);

    MatrixXf emb = newU * newS.cwiseSqrt().asDiagonal();

    emb = l2Normalize(emb);
    return emb;
}

MatrixXf getEmbbeddingViaSvd(SMatrixXf &data, int rank){
    RedSVD redsvd;
    redsvd.run(data, rank);
    MatrixXf emb = redsvd.matrixU() * redsvd.singularValues().cwiseSqrt().asDiagonal();
    emb = l2Normalize(emb);
    return emb;
}

MatrixXf getEmbbeddingViaDenseSvd(MatrixXf &data, int rank){
    // Eigen::JacobiSVD<Eigen::MatrixXf> svdOfC(data, Eigen::ComputeThinU | Eigen::ComputeThinV);
    // Eigen::BDCSVD<Eigen::MatrixXf> svdOfC(data, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::BDCSVD<Eigen::MatrixXf> svdOfC(data, Eigen::ComputeThinU);
    MatrixXf emb = svdOfC.matrixU() * svdOfC.singularValues().cwiseSqrt().asDiagonal();
    emb = l2Normalize(emb);
    return emb;
}

MatrixXf getSparseEmbedding(SMatrixXf & A, int rank, int num_iter){
    time_t t1 = time(NULL);
    // cout << "number of nnz: "<< A.nonZeros() <<endl;
    int row = A.rows(), col = A.cols();
    SMatrixXf B = l1Normalize(A);
    SMatrixXf C = B.transpose();
    SMatrixXf D(col, col), E(row, col), F(row, col);
    for (int i = 0; i < row; ++i){
        D.insert(i, i) = pow(C.row(i).sum(), 0.75);
    }

    D = D / D.sum();
    E = A * D;

    B = validate(B);
    E = validate(E);

    B = smfLog(B);
    E = smfLog(E);
    F = B - E;
    cout << "preprocess time: "<< (time(NULL) - t1 + 0.0) << endl;
    // printSmf(F);
    //cout << "number of nnz: "<< F.nonZeros() <<endl;

    MatrixXf emb = getEmbbeddingViaSvd(F, rank);
    //MatrixXf emb = randomizedSvd(F, rank, num_iter);
    return emb;
}

MatrixXf getSpectralEmbedding(SMatrixXf & A, MatrixXf & a, int step, float theta, float mu){
    time_t t1 = time(NULL);
    cout << "Chebyshev series --------------- " << endl;
    if (step==1) return a;
    int num_node = a.rows(), rank = a.cols();
    SMatrixXf I(num_node, num_node);
    for (int i = 0; i < num_node; ++i)
        I.insert(i, i) = 1;
    A = A + I;
    SMatrixXf B = l1Normalize(A);
    SMatrixXf L = I - B;
    SMatrixXf M = L - mu * I;
    // cout << "number of nnz: "<< M.nonZeros() <<endl;
    // printSmf(M);

    MatrixXf Lx0 = a;
    MatrixXf Lx1 = M * a, Lx2;
    Lx1 = 0.5 * M * Lx1 - a;

    MatrixXf conv = bessel(0, theta)* Lx0;
    conv -= 2 * bessel(1, theta)* Lx1;
    for(int i=2; i<step; i++){
        Lx2 = M * Lx1;
        Lx2 = (M * Lx2 - 2 * Lx1) - Lx0;

        if (i % 2 == 0)
            conv += 2 * bessel(i, theta) * Lx2;
        else
            conv -= 2 * bessel(i, theta) * Lx2;
        Lx0 = Lx1;
        Lx1 = Lx2;
        cout << "Bessell time: " << i <<"\t"<< (time(NULL) - t1 + 0.0) << endl;
    }
    MatrixXf F = A * (a - conv);
    cout << "Chebyshev time: "<< (time(NULL) - t1 + 0.0) << endl;
    time_t t2 = time(NULL);

    MatrixXf emb = getEmbbeddingViaDenseSvd(F, rank);
    cout << "dense svd time: "<< (time(NULL) - t2 + 0.0) << endl;
    return emb;
}

int main(int argc, char** argv)
{
    gflags::ParseCommandLineFlags(&argc, &argv, true);
    time_t start_time = time(NULL);
    Eigen::setNbThreads(FLAGS_num_thread);

    SMatrixXf A = readGraph(FLAGS_filename, FLAGS_num_node);
    time_t t1 = time(NULL);
    cout << "Running time of read graph: " << (t1 - start_time + 0.0)<< endl;

    MatrixXf feature = getSparseEmbedding(A, FLAGS_num_rank, FLAGS_num_iter);
    time_t t2 = time(NULL);
    cout << "Running time of get sparse embedding: " << (t2 - t1 + 0.0) << endl;

    MatrixXf embedding = getSpectralEmbedding(A, feature, FLAGS_num_step, FLAGS_theta, FLAGS_mu);
    time_t t3 = time(NULL);
    cout << "Running time of get spectral embedding: " << (t3 - t2 + 0.0)  << endl;
    cout << "Running time of ProNE: " << (t3 - start_time + 0.0) << endl;
    saveEmbedding(feature, FLAGS_emb1);
    saveEmbedding(feature, FLAGS_emb2);

}