/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021,
 *  Max Planck Institute for Intelligent Systems (MPI-IS).
 *  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 the MPI-IS 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 AND CONTRIBUTORS
 *  "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 THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS 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.
 *********************************************************************/

/* Author: Andreas Orthey */

#include <ompl/base/spaces/special/KleinBottleStateSpace.h>
#include <ompl/tools/config/MagicConstants.h>
#include <cstring>
#include <cmath>
#include <boost/math/constants/constants.hpp>

using namespace boost::math::double_constants;  // pi
using namespace ompl::base;

KleinBottleStateSampler::KleinBottleStateSampler(const StateSpace *space) : StateSampler(space)
{
}

void KleinBottleStateSampler::sampleUniform(State *state)
{
    bool acceptedSampleFound = false;
    while (!acceptedSampleFound)
    {
        const double u = rng_.uniformReal(0, pi);
        const double v = rng_.uniformReal(-pi, pi);

        // NOTE: The idea here is that to compute the norm of the gradient at each
        // point of the surface (i.e. the gradient of the coordinate mapping from
        //(u,v) to (x,y,z)). To get the norm, we divide by the maximum norm of the
        // gradient over the whole surface. This gives a number between [0,1]. We
        // then do rejection sampling, by choosing a random number in [0,1] and
        // accept if the norm is larger than this random number. Surface elements
        // with a high curvature will have a small norm and will therefore be
        // penalized under this method (i.e. rejected more often).
        // See also: https://mathematica.stackexchange.com/questions/148693/generating-random-points-on-a-kleins-bottle

        // NOTE: Automatic differential via sympy script
        const double cu = std::cos(u);
        const double cv = std::cos(v);
        const double su = std::sin(u);
        const double sv = std::sin(v);
        const double cu3 = std::pow(cu, 3);
        const double cu5 = std::pow(cu, 5);
        const double cu6 = std::pow(cu, 6);
        const double cu7 = std::pow(cu, 7);
        const double cu8 = std::pow(cu, 8);

        const double su2 = std::pow(su, 2);
        const double su3 = std::pow(su, 3);
        const double su4 = std::pow(su, 4);
        const double su5 = std::pow(su, 5);
        const double su6 = std::pow(su, 6);
        const double su7 = std::pow(su, 7);
        const double su8 = std::pow(su, 8);

        const double aprime = (64.0 * su8 - 128.0 * su6 + 60.0 * su4 + 0.4 * su * cv - (1.0 / 6.0) * cu * cv -
                               0.5 * std::cos(3 * u) * cv);

        const double a = (-aprime * cv + (2.0 / 3.0) * sv * sv * cu * std::cos(2.0 * u));

        const double bprime =
            ((26 + 2.0 / 3.0) * su7 * cv - 55.0 * su5 * cv - (37 + 1.0 / 3.0) * su3 * cu6 * cv + 28.0 * su3 * cv +
             (10 + 2.0 / 3.0) * su * cu8 * cv - (10 + 2.0 / 3.0) * su * cu6 * cv - 4.0 * std::sin(2.0 * u) +
             22.4 * cu7 * cv - 35.2 * cu5 * cv + 12.2 * cu3 * cv + 0.6 * cu * cv);

        const double cprime = ((5 + 1.0 / 3.0) * su5 * cu + 3.2 * su4 - (10 + 2.0 / 3.0) * su3 * cu - 6.4 * su2 +
                               2.5 * std::sin(2 * u) + 3.0);

        const double b = (((1.0 / 3.0) * std::sin(2.0 * u) + 0.4) * bprime * cu - cprime * aprime * su3);

        const double c = ((5.0 / 6.0) * std::sin(2.0 * u) + 1);

        const double d = (-((1.0 / 3.0) * std::sin(2.0 * u) + 0.4) * bprime * cv +
                          (2.0 / 3.0) * cprime * su3 * sv * sv * std::cos(2.0 * u));

        double s = std::sqrt(a * a * (0.16 * c * c) + b * b * sv * sv + d * d);

        if (s > gMax_)
        {
            OMPL_ERROR("Norm of gradient (%.10f) larger than maximum norm (%.10f).", s, gMax_);
            throw "Wrong norm error.";
        }
        s = s / gMax_;

        const double mu = rng_.uniformReal(0, 1);
        if (mu <= s)
        {
            auto *K = state->as<KleinBottleStateSpace::StateType>();
            K->setUV(u, v);
            acceptedSampleFound = true;
        }
    }
}

void KleinBottleStateSampler::sampleUniformNear(State *state, const State *near, double distance)
{
    auto *K = state->as<KleinBottleStateSpace::StateType>();
    const auto *Knear = near->as<KleinBottleStateSpace::StateType>();
    K->setU(rng_.uniformReal(Knear->getU() - distance, Knear->getU() + distance));
    K->setV(rng_.uniformReal(Knear->getV() - distance, Knear->getV() + distance));
    space_->enforceBounds(state);
}

void KleinBottleStateSampler::sampleGaussian(State *state, const State *mean, double stdDev)
{
    auto *K = state->as<KleinBottleStateSpace::StateType>();
    const auto *Kmean = mean->as<KleinBottleStateSpace::StateType>();
    K->setU(rng_.gaussian(Kmean->getU(), stdDev));
    K->setV(rng_.gaussian(Kmean->getV(), stdDev));

    space_->enforceBounds(state);
}

KleinBottleStateSpace::KleinBottleStateSpace()
{
    setName("KleinBottle" + getName());
    type_ = STATE_SPACE_KLEIN_BOTTLE;

    // We model the Klein bottle as a regular cylinder, but where both ends are
    // glued inversely together. For more information, check out the
    // wikipedia article: https://en.wikipedia.org/wiki/Klein_bottle.
    // Both interpolation and distance computation have to take
    // the gluing into account when crossing over the boundary.
    // ------<-------
    // |            |
    // |            |
    // v            v u-dimension (0 to pi)
    // |            |
    // |            |
    // ------>-------
    //  v-dimension (0 to 2*pi)
    //
    //  Gluing:
    // u=pi+0.001:  0 ----------- -pi pi ---------- 0
    // u=0       :  -pi ----------- 0 0 ---------- pi

    StateSpacePtr R1(std::make_shared<RealVectorStateSpace>(1));
    R1->as<RealVectorStateSpace>()->setBounds(0, pi);

    StateSpacePtr SO2(std::make_shared<SO2StateSpace>());

    addSubspace(R1, 1.0);
    addSubspace(SO2, 1.0);

    lock();
}

StateSamplerPtr KleinBottleStateSpace::allocDefaultStateSampler() const
{
    return std::make_shared<KleinBottleStateSampler>(this);
}

double KleinBottleStateSpace::distance(const State *state1, const State *state2) const
{
    const double u1 = state1->as<KleinBottleStateSpace::StateType>()->getU();
    const double u2 = state2->as<KleinBottleStateSpace::StateType>()->getU();

    const double diffU = u2 - u1;

    if (std::abs(diffU) <= 0.5 * pi)
    {
        return CompoundStateSpace::distance(state1, state2);
    }
    else
    {
        const double d_u = pi - std::abs(diffU);

        const double v1 = state1->as<KleinBottleStateSpace::StateType>()->getV();
        double v2 = state2->as<KleinBottleStateSpace::StateType>()->getV();

        // reverse v2 (valid for both directions)
        v2 = (v2 > 0.0 ? pi - v2 : -pi - v2);

        double d_v = std::abs(v2 - v1);
        d_v = (d_v > pi) ? 2.0 * pi - d_v : d_v;

        double dist = d_u + d_v;

        return dist;
    }
}

void KleinBottleStateSpace::interpolate(const State *from, const State *to, double t, State *state) const
{
    const double u1 = from->as<KleinBottleStateSpace::StateType>()->getU();
    const double u2 = to->as<KleinBottleStateSpace::StateType>()->getU();

    double diffU = u2 - u1;

    if (std::abs(diffU) <= 0.5 * pi)
    {
        // interpolate as if it would be a cylinder
        CompoundStateSpace::interpolate(from, to, t, state);
    }
    else
    {
        // Interpolate along u-dimension
        if (diffU > 0.0)
        {
            diffU = pi - diffU;
        }
        else
        {
            diffU = -pi - diffU;
        }

        double u = u1 - diffU * t;

        bool crossed = false;
        if (u > pi)
        {
            u -= pi;
            crossed = true;
        }
        else if (u < 0.0)
        {
            u += pi;
            crossed = true;
        }

        state->as<KleinBottleStateSpace::StateType>()->setU(u);

        double v1 = from->as<KleinBottleStateSpace::StateType>()->getV();
        double v2 = to->as<KleinBottleStateSpace::StateType>()->getV();

        // If we crossed the gluing, we need to invert the "from"-state, otherwise
        // we need to invert the "to"-state (similar to default SO2 interpolation)
        if (crossed)
        {
            v1 = (v1 > 0.0 ? pi - v1 : -pi - v1);
        }
        else
        {
            v2 = (v2 > 0.0 ? pi - v2 : -pi - v2);
        }

        double diffV = v2 - v1;
        double v = 0;

        if (std::abs(diffV) <= pi)
        {
            v = v1 + diffV * t;
        }
        else
        {
            if (diffV > 0.0)
                diffV = 2.0 * pi - diffV;
            else
                diffV = -2.0 * pi - diffV;
            v = v1 - diffV * t;

            if (v > pi)
                v -= 2.0 * pi;
            else if (v < -pi)
                v += 2.0 * pi;
        }

        state->as<KleinBottleStateSpace::StateType>()->setV(v);
    }
}

State *KleinBottleStateSpace::allocState() const
{
    auto *state = new StateType();
    allocStateComponents(state);
    return state;
}

Eigen::Vector3f KleinBottleStateSpace::toVector(const State *state) const
{
    // Formula from https://en.wikipedia.org/wiki/Klein_bottle#Bottle_shape
    const auto *s = state->as<KleinBottleStateSpace::StateType>();
    const float u = s->getU();
    const float v = s->getV() + pi;  // NOTE: SO2 state space has bounds [-pi, +pi]

    assert(u >= 0.0);
    assert(u <= pi);
    assert(v >= 0.0);
    assert(v <= 2 * pi);

    double cu = std::cos(u);
    double cv = std::cos(v);
    double su = std::sin(u);
    double sv = std::sin(v);
    double cu2 = std::pow(cu, 2);
    double cu3 = std::pow(cu, 3);
    double cu4 = std::pow(cu, 4);
    double cu5 = std::pow(cu, 5);
    double cu6 = std::pow(cu, 6);
    double cu7 = std::pow(cu, 7);

    double a = 3 * cv - 30 * su + 90 * cu4 * su - 60 * cu6 * su + 5 * cu * cv * su;

    Eigen::Vector3f q;
    q[0] = -2.0 / 15.0 * cu * a;

    double b = 3 * cv - 3 * cu2 * cv - 48 * cu4 * cv + 48 * cu6 * cv - 60 * su + 5 * cu * cv * su - 5 * cu3 * cv * su -
               80 * cu5 * cv * su + 80 * cu7 * cv * su;

    q[1] = -1.0 / 15.0 * su * b;

    q[2] = 2.0 / 15.0 * (3 + 5 * cu * su) * sv;

    return q;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Peter Caspers
 Copyright (C) 2015 Roland Lichters

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file betaeta.hpp
    \brief Hagan / Woodward beta-eta model
*/

#ifndef quantlib_model_betaeta_hpp
#define quantlib_model_betaeta_hpp

#include <ql/experimental/models/betaetacore.hpp>
#include <ql/models/model.hpp>
#include <ql/indexes/iborindex.hpp>
#include <ql/indexes/swapindex.hpp>
#include <ql/instruments/vanillaswap.hpp>

#include <boost/unordered_map.hpp>

namespace QuantLib {

/*! cf. Hagan, Woodward: Markov interest rate models,
    Applied Mathematical Finance 6, 233–260 (1999)

    We assume a reflecting barrier at x = -1/beta (cf.
    the last paragraph on p. 241).

    We assume a piecewise constant reversion \kappa and
    set \lambda(t) := (1-exp(-\kappa*t))/\kappa
    note that these are effective (integrated) rather
    than forward-forward reversions though
*/

// TODO there is a big overlap with the Gaussian1d model interface
// refactor this, i.e. create a common base class and build the
// engines on top of this

class BetaEta : public TermStructureConsistentModel,
                public CalibratedModel,
                public LazyObject {
  public:
    // constant mean reversion
    BetaEta(const Handle<YieldTermStructure> &termStructure,
            const std::vector<Date> &volstepdates,
            const std::vector<Real> &volatilities, const Real reversion,
            const Real beta, const Real eta);
    // piecewise mean reversion (with same step dates as volatilities)
    BetaEta(const Handle<YieldTermStructure> &termStructure,
            const std::vector<Date> &volstepdates,
            const std::vector<Real> &volatilities,
            const std::vector<Real> &reversions, const Real beta,
            const Real eta);
    // constant mean reversion with floating model data
    BetaEta(const Handle<YieldTermStructure> &termStructure,
            const std::vector<Date> &volstepdates,
            const std::vector<Handle<Quote> > &volatilities,
            const Handle<Quote> reversion, const Handle<Quote> beta,
            const Handle<Quote> eta);
    // piecewise mean reversion with floating model data
    BetaEta(const Handle<YieldTermStructure> &termStructure,
            const std::vector<Date> &volstepdates,
            const std::vector<Handle<Quote> > &volatilities,
            const std::vector<Handle<Quote> > &reversions,
            const Handle<Quote> beta, const Handle<Quote> eta);

    const Real numeraire(const Time t, const Real x = 0.0,
                         const Handle<YieldTermStructure> &yts =
                             Handle<YieldTermStructure>()) const;

    const Real zerobond(const Time T, const Time t = 0.0, const Real x = 0.0,
                        const Handle<YieldTermStructure> &yts =
                            Handle<YieldTermStructure>()) const;

    const Real numeraire(const Date &referenceDate, const Real y = 0.0,
                         const Handle<YieldTermStructure> &yts =
                             Handle<YieldTermStructure>()) const;
    const Real zerobond(const Date &maturity,
                        const Date &referenceDate = Null<Date>(),
                        const Real x = 0.0,
                        const Handle<YieldTermStructure> &yts =
                            Handle<YieldTermStructure>()) const;

    const Real forwardRate(const Date &fixing,
                           const Date &referenceDate = Null<Date>(),
                           const Real x = 0.0,
                           boost::shared_ptr<IborIndex> iborIdx =
                               boost::shared_ptr<IborIndex>()) const;

    const Real swapRate(const Date &fixing, const Period &tenor,
                        const Date &referenceDate = Null<Date>(),
                        const Real x = 0.0,
                        boost::shared_ptr<SwapIndex> swapIdx =
                            boost::shared_ptr<SwapIndex>()) const;

    const Real swapAnnuity(const Date &fixing, const Period &tenor,
                           const Date &referenceDate = Null<Date>(),
                           const Real x = 0.0,
                           boost::shared_ptr<SwapIndex> swapIdx =
                               boost::shared_ptr<SwapIndex>()) const;

    const Array &reversion() const { return reversion_.params(); }
    const Array &volatility() const { return sigma_.params(); }
    const Real beta() const { return pBeta_.params()[0]; }
    const Real eta() const { return pEta_.params()[0]; }

    /*! Generates a grid of values for the state variable $x$
       at time $T$ conditional on $x(t)=x$, covering stdDevs
       standard deviations assuming an approximate variance
       $\int_t^T \alpha^2$ for $x$. The grid consists of
       2*gridPoints+1 points */
    const Disposable<Array> xGrid(const Real stdDevs, const int gridPoints,
                                  const Real T = 1.0, const Real t = 0,
                                  const Real x = 0) const;

    /*! integrates f against the conditional density $x(t)|x(t0)=x0$
        covering stdDevs approximate standard deviations in the same
        sense as in the xGrid implementation */
    const Real integrate(const Real stdDevs,
                         const boost::function<Real(Real)> &f, const Real t0,
                         const Real x0, const Real t) const;

    /*! for testing purposes we can switch off the usage of tabulated values */
    const void useTabulation(const bool useTabulation) {
        if (useTabulation_ != useTabulation) {
            useTabulation_ = useTabulation;
            update();
        }
    }

    // calibration constraints
    // TODO what is of practical use here ?

    Disposable<std::vector<bool> > MoveVolatility(Size i) {
        QL_REQUIRE(i < volatilities_.size(),
                   "volatility with index " << i << " does not exist (0..."
                                            << volatilities_.size() - 1 << ")");
        std::vector<bool> res(reversions_.size() + volatilities_.size() + 2,
                              true);
        res[reversions_.size() + i] = false;
        return res;
    }

    // With fixed reversion, beta and eta calibrate the volatilities
    // one by one to the given helpers. The same comments as in the
    // corresonding method in Gsr hold.
    void calibrateVolatilitiesIterative(
        const std::vector<boost::shared_ptr<CalibrationHelper> > &helpers,
        OptimizationMethod &method, const EndCriteria &endCriteria,
        const Constraint &constraint = Constraint(),
        const std::vector<Real> &weights = std::vector<Real>()) {

        for (Size i = 0; i < helpers.size(); i++) {
            std::vector<boost::shared_ptr<CalibrationHelper> > h(1, helpers[i]);
            calibrate(h, method, endCriteria, constraint, weights,
                      MoveVolatility(i));
        }
    }

  private:
    void generateArguments() { notifyObservers(); }

    // see Gaussian1dModel

    struct CachedSwapKey {
        const boost::shared_ptr<SwapIndex> index;
        const Date fixing;
        const Period tenor;
        const bool operator==(const CachedSwapKey &o) const {
            return index->name() == o.index->name() && fixing == o.fixing &&
                   tenor == o.tenor;
        }
    };

    struct CachedSwapKeyHasher
        : std::unary_function<CachedSwapKey, std::size_t> {
        std::size_t operator()(CachedSwapKey const &x) const {
            std::size_t seed = 0;
            boost::hash_combine(seed, x.index->name());
            boost::hash_combine(seed, x.fixing.serialNumber());
            boost::hash_combine(seed, x.tenor.length());
            boost::hash_combine(seed, x.tenor.units());
            return seed;
        }
    };

    typedef boost::unordered_map<CachedSwapKey, boost::shared_ptr<VanillaSwap>,
                                 CachedSwapKeyHasher> CacheType;

    mutable CacheType swapCache_;

    // retrieve underlying swap from cache if possible, otherwise
    // create it and store it in the cache
    boost::shared_ptr<VanillaSwap>
    underlyingSwap(const boost::shared_ptr<SwapIndex> &index,
                   const Date &expiry, const Period &tenor) const {

        CachedSwapKey k = {index, expiry, tenor};
        CacheType::iterator i = swapCache_.find(k);
        if (i == swapCache_.end()) {
            boost::shared_ptr<VanillaSwap> underlying =
                index->clone(tenor)->underlyingSwap(expiry);
            swapCache_.insert(std::make_pair(k, underlying));
            return underlying;
        }
        return i->second;
    }

    void update() { LazyObject::update(); }

    void performCalculations() const {
        evaluationDate_ = Settings::instance().evaluationDate();
        enforcesTodaysHistoricFixings_ =
            Settings::instance().enforcesTodaysHistoricFixings();
        updateTimes();
    }

    void updateTimes() const;
    void updateVolatility();
    void updateReversion();
    void updateBeta();
    void updateEta();

    void initialize();

    Parameter &reversion_, &sigma_, &pBeta_, &pEta_;
    std::vector<Handle<Quote> > volatilities_;
    std::vector<Handle<Quote> > reversions_;
    Handle<Quote> beta_;
    Handle<Quote> eta_;
    mutable Real betaLink_, etaLink_; // redundant, just used as a link to
                                      // the core computation class

    std::vector<Date> volstepdates_; // these are shared between volatilities
                                     // and reversions in case of piecewise
                                     // reversions
    mutable std::vector<Time> volsteptimes_;
    mutable Array volsteptimesArray_; // redundant, just used as a link to
                                      // the core computation class

    mutable Date evaluationDate_;
    mutable bool enforcesTodaysHistoricFixings_;

    boost::shared_ptr<BetaEtaCore> core_;
    boost::shared_ptr<Integrator> integrator_, integrator2_;

    class integrand {
      public:
        integrand(const Real t0, const Real x0, const Real t,
                  const boost::function<Real(Real)> &f, const BetaEtaCore &core)
            : t0_(t0), x0_(x0), t_(t), f_(f), core_(core) {}
        Real operator()(Real x) const {
            return f_(x) * core_.p(t0_, x0_, t_, x);
        }

      private:
        const Real t0_, x0_, t_;
        const boost::function<Real(Real)> &f_;
        const BetaEtaCore &core_;
    };
    friend class integrand;

    bool useTabulation_; // for testing, normally it should be true

    struct VolatilityObserver : public Observer {
        VolatilityObserver(BetaEta *p) : p_(p) {}
        void update() { p_->updateVolatility(); }
        BetaEta *p_;
    };
    struct ReversionObserver : public Observer {
        ReversionObserver(BetaEta *p) : p_(p) {}
        void update() { p_->updateReversion(); }
        BetaEta *p_;
    };
    struct BetaObserver : public Observer {
        BetaObserver(BetaEta *p) : p_(p) {}
        void update() { p_->updateBeta(); }
        BetaEta *p_;
    };
    struct EtaObserver : public Observer {
        EtaObserver(BetaEta *p) : p_(p) {}
        void update() { p_->updateEta(); }
        BetaEta *p_;
    };

    boost::shared_ptr<VolatilityObserver> volatilityObserver_;
    boost::shared_ptr<ReversionObserver> reversionObserver_;
    boost::shared_ptr<BetaObserver> betaObserver_;
    boost::shared_ptr<EtaObserver> etaObserver_;
};

// implementation

inline const Real
BetaEta::numeraire(const Date &referenceDate, const Real x,
                   const Handle<YieldTermStructure> &yts) const {

    return numeraire(termStructure()->timeFromReference(referenceDate), x, yts);
}

inline const Real
BetaEta::zerobond(const Date &maturity, const Date &referenceDate, const Real x,
                  const Handle<YieldTermStructure> &yts) const {

    return zerobond(termStructure()->timeFromReference(maturity),
                    referenceDate != Null<Date>()
                        ? termStructure()->timeFromReference(referenceDate)
                        : 0.0,
                    x, yts);
}

inline const Real BetaEta::integrate(const Real stdDevs,
                                     const boost::function<Real(Real)> &f,
                                     const Real t0, const Real x0,
                                     const Real t) const {
    Real s = std::sqrt(core_->tau(t0, t));
    integrand phi(t0, x0, t, f, *core_);
    Real result;
    // left integration bound should be greater or equal to barrier
    Real a = std::max(x0 - stdDevs * s, -1.0 / beta_->value());
    Real b = x0 + stdDevs * s;
    try {
        result = (*integrator_)(phi, a, b);
    } catch (QuantLib::Error) {
        result = (*integrator2_)(phi, a, b);
    }

    // singular term

    result +=
        core_->prob_y_0(t0, x0, t, useTabulation_) * f(-1.0 / beta_->value());
    return result;
}

} // namespace QuantLib

#endif


//######################################################################
//#   Refiner Module
//#
//#   Copyright (C) 2020 Siemens AG
//#   SPDX-License-Identifier: MIT
//#   Author 2020: This module has been developed by
//#                Roman Kaskman under supervision of Slobodan Ilic
//#######################################################################

#include <opencv/cv.hpp>
#include "optimizer.h"
#include "ceres/ceres.h"
#include "ceres/rotation.h"
#include "util.h"
#include <Eigen/Dense>
#include "correspondence_finder.h"
#include "frame_payload.h"
#include <utility>

#define GLOG_NO_ABBREVIATED_SEVERITIES

using namespace ceres;
using namespace std;

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;

struct ModelPoseFunctor {
	ModelPoseFunctor(Eigen::Vector2d pixel_point,  Eigen::Vector3d world_point, Eigen::Matrix3d intrinsics,
		double* scene_inverse_angle_axis, double* scene_pose_inverse_translation, Eigen::Vector2d rgb_edge_normal, double std_dev)
		:pixel_point(std::move(pixel_point)), world_point(std::move(world_point)), intrinsics(std::move(intrinsics)),
		scene_inverse_angle_axis(scene_inverse_angle_axis), scene_pose_inverse_translation(scene_pose_inverse_translation),
		rgb_edge_normal(std::move(rgb_edge_normal)), std_dev(std_dev) {}

	template <typename T>  bool operator()(const T* const rotation_to_optimize, const T* const translation_to_optimize, T* residual) const {

		T p[3];

		p[0] = T(world_point[0]);
		p[1] = T(world_point[1]);
		p[2] = T(world_point[2]);

		ceres::AngleAxisRotatePoint(rotation_to_optimize, p, p);

		p[0] += translation_to_optimize[0];
		p[1] += translation_to_optimize[1];
		p[2] += translation_to_optimize[2];

		T scene_inverse_angle_axis_t[3];
		scene_inverse_angle_axis_t[0] = T(scene_inverse_angle_axis[0]);
		scene_inverse_angle_axis_t[1] = T(scene_inverse_angle_axis[1]);
		scene_inverse_angle_axis_t[2] = T(scene_inverse_angle_axis[2]);

		ceres::AngleAxisRotatePoint(scene_inverse_angle_axis_t, p, p);

		T scene_inverse_translation_t[3];
		scene_inverse_translation_t[0] = T(scene_pose_inverse_translation[0]);
		scene_inverse_translation_t[1] = T(scene_pose_inverse_translation[1]);
		scene_inverse_translation_t[2] = T(scene_pose_inverse_translation[2]);

		p[0] += scene_inverse_translation_t[0];
		p[1] += scene_inverse_translation_t[1];
		p[2] += scene_inverse_translation_t[2];

		T predicted_x = ((T(intrinsics(0, 0))*p[0]) / p[2]) + T(intrinsics(0, 2));
		T predicted_y = ((T(intrinsics(1, 1))*p[1]) / p[2]) + T(intrinsics(1, 2));

		residual[0] = ((predicted_x - T(pixel_point[0])) * T(rgb_edge_normal[0]) +
			(predicted_y - T(pixel_point[1])) * T(rgb_edge_normal[1])) / T(std_dev);
		return true;
	}

	Eigen::Vector2d pixel_point;
	Eigen::Vector3d world_point;

	Eigen::Matrix3d intrinsics;
	double* scene_inverse_angle_axis;
	double* scene_pose_inverse_translation;
	Eigen::Vector2d rgb_edge_normal;
	double std_dev;
};

Eigen::Matrix4d optimize_model_pose(const vector<FramePayload> &frame_payloads, const Eigen::Matrix4d &model_pose, const Eigen::Matrix3d &intrinsics, double residuals_std_dev)
{
	double* angle_axis_to_optimize = new double[3];
	double* translation_to_optimize = new double[3];

	Eigen::Matrix3d model_rotation = model_pose.block<3, 3>(0, 0);
	Eigen::Vector3d model_translation = model_pose.block<3, 1>(0, 3);

	ceres::RotationMatrixToAngleAxis(model_rotation.data(), angle_axis_to_optimize);
	memcpy(translation_to_optimize, model_translation.data(), 3 * sizeof(double));

	std::cout << "angle axis : " << angle_axis_to_optimize[0]
		<< " -> " << angle_axis_to_optimize[1]
		<< " -> " << angle_axis_to_optimize[2]
		<< ", translation " << translation_to_optimize[0]
		<< " -> " << translation_to_optimize[1]
		<< " -> " << translation_to_optimize[2] << "\n";

	Problem problem;

	vector<double*> angle_axis_scene_invere_rotations;
	vector<double*> scene_invere_translations;
	cout << "Residuals std dev: " << residuals_std_dev << endl;

	for (const FramePayload &payload : frame_payloads)
	{
		const Correspondence &correspondence = payload.correspondence;

		Eigen::Matrix4d scene_pose_inverse = payload.scene_pose.inverse();

		Eigen::Matrix3d scene_pose_inverse_rotation = scene_pose_inverse.block<3, 3>(0, 0);
		Eigen::Vector3d scene_pose_inverse_translation = scene_pose_inverse.block<3, 1>(0, 3);

		double* angle_axis_scene_inverse = new double[3];
		ceres::RotationMatrixToAngleAxis(scene_pose_inverse_rotation.data(), angle_axis_scene_inverse);

		double* scene_pose_inverse_translation_array = new double[3];
		memcpy(scene_pose_inverse_translation_array, scene_pose_inverse_translation.data(), 3 * sizeof(double));

		angle_axis_scene_invere_rotations.push_back(angle_axis_scene_inverse);
		scene_invere_translations.push_back(scene_pose_inverse_translation_array);

		for (int i = 0; i < correspondence.get_number_of_correspondences(); i++) {
			auto pixel_point = correspondence.corresponding_points[i].cast<double>();
			auto world_point = correspondence.world_points[i].cast<double>();

			Eigen::Vector2d rgb_edge_normal = correspondence.get_rgb_normal(i).cast<double>();

			CostFunction *cost_function =
				new AutoDiffCostFunction<ModelPoseFunctor, 1, 3, 3>(new ModelPoseFunctor(pixel_point, world_point, intrinsics,
					angle_axis_scene_inverse, scene_pose_inverse_translation_array,
					rgb_edge_normal, residuals_std_dev));

			TukeyLoss* loss_function = new TukeyLoss(4.365);
			problem.AddResidualBlock(cost_function, loss_function, angle_axis_to_optimize, translation_to_optimize);
		}
	}

	// run the solver
	Solver::Options options;
	options.minimizer_progress_to_stdout = false;
	options.max_num_iterations = 100;
	Solver::Summary summary;
	ceres::Solve(options, &problem, &summary);
	std::cout << summary.BriefReport() << "\n";

	std::cout << "optimized angle axis : " << angle_axis_to_optimize[0]
		<< " -> " << angle_axis_to_optimize[1]
		<< " -> " << angle_axis_to_optimize[2]
		<< ", optimized translation " << translation_to_optimize[0]
		<< " -> " << translation_to_optimize[1]
		<< " -> " << translation_to_optimize[2] << "\n";

	double optimal_rotation_matrix_array[9];
	ceres::AngleAxisToRotationMatrix(angle_axis_to_optimize, optimal_rotation_matrix_array);

	Eigen::Matrix3d rotation_matrix = Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::ColMajor>>(optimal_rotation_matrix_array);
	Eigen::Vector3d translation_vector = Eigen::Map<Eigen::Matrix<double, 3, 1, Eigen::ColMajor>>(translation_to_optimize);

	delete[] angle_axis_to_optimize;
	delete[] translation_to_optimize;

	for (auto &arr : angle_axis_scene_invere_rotations) {
		delete[] arr;
	}

	for (auto &arr : scene_invere_translations) {
		delete[] arr;
	}

	return create_transformation(rotation_matrix, translation_vector);
}


#include <NTL/ZZ.h>
#include <vector>
#include "constants.h"
#include "generalhelpers.h"

namespace CryptoHelpers
{
    std::vector<NTL::ZZ> g_roundKeys(4);
    std::vector<unsigned long long> g_roundKeysInt(4);

    void RoundFunction(NTL::ZZ& input, const NTL::ZZ& key)
    {
        input ^= key;

        NTL::ZZ newRightHalf(0);

        for (auto byteIndex = 0u; byteIndex < 4; ++byteIndex)
        {
            unsigned char byte = 0;
            NTL::BytesFromZZ(&byte, (input >> (8 * byteIndex)) & 0xFF, 1);
            newRightHalf |= SBOX[byte] << (8 * byteIndex);
        }
        auto shift = (newRightHalf >> 4) & 0xFFF8000;
        shift <<= 4;
        newRightHalf <<= 13;
        newRightHalf |= shift;
        newRightHalf = And32Bits(newRightHalf);

        input = 0;
        input |= newRightHalf;
    }

    template <typename T>
    T reverse(T n, size_t b = sizeof(T) * 8)
    {
        //        assert(b <= std::numeric_limits<T>::digits);

        T rv = 0;

        for (size_t i = 0; i < b; ++i, n >>= 1)
            rv = (rv >> 1) | (n & 0x01);

        return rv;
    }

    void RoundFunction(const unsigned long long& _input, const unsigned long long& key, unsigned long long& result, bool b = true)
    {
        auto input = _input ^ key;

        unsigned long long newRightHalf(0);

        for (auto byteIndex = 0u; byteIndex < 4; ++byteIndex)
        {
            //newRightHalf <<= 8;
            newRightHalf |= SBOX[(input >> (8 * byteIndex)) & 0xFF] << (8 * byteIndex);
        }

        auto shift = newRightHalf & 0xFFF80000;
        shift >>= 19;
        newRightHalf <<= 13;
        newRightHalf |= shift;
        newRightHalf = (newRightHalf) & 0xFFFFFFFF;

        result = newRightHalf;
    }

    void GenerateRoundKeys(const NTL::ZZ& key)
    {
        g_roundKeys[0] = And32Bits(key);
        g_roundKeys[1] = And32Bits(key >> 32);
        g_roundKeys[2] = Xor32Bits(g_roundKeys[0]);
        g_roundKeys[3] = Xor32Bits(g_roundKeys[1]);
    }

    void GenerateRoundKeys(const unsigned long long& key)
    {
        g_roundKeysInt[0] = (key >> 32) & 0xFFFFFFFF;
        g_roundKeysInt[1] = key & 0xFFFFFFFF;
        ReverseBytes(4,g_roundKeysInt[0]);
        ReverseBytes(4,g_roundKeysInt[1]);
        g_roundKeysInt[2] = (g_roundKeysInt[1] ^ 0xFFFFFFFF);
        g_roundKeysInt[3] = (g_roundKeysInt[0] ^ 0xFFFFFFFF);
    }

    void GenerateKeys(const NTL::ZZ* _key = nullptr)
    {
        if (!_key)
        {
            auto rand = NTL::GetCurrentRandomStream();
            std::vector<unsigned char> randomValue(8);
            rand.get(randomValue.data(), 8);
            NTL::ZZ key(0);
            NTL::ZZFromBytes(key, randomValue.data(), 8);
            GenerateRoundKeys(key);
            return;
        }
        GenerateRoundKeys(*_key);
    }

    void GenerateKeys(const unsigned long long* _key = nullptr)
    {
        if (!_key)
        {
            //auto rand = NTL::GetCurrentRandomStream();
            //std::vector<unsigned char> randomValue(8);
            //rand.get(randomValue.data(), 8);
            //NTL::ZZ key(0);
            //NTL::ZZFromBytes(key, randomValue.data(), 8);
            //GenerateRoundKeys(key);
            return;
        }
        GenerateRoundKeys(*_key);
    }

    void Encrypt(const NTL::ZZ & source, NTL::ZZ & target, const NTL::ZZ* key = nullptr)
    {
        GenerateKeys(key);
        auto rightHalf = And32Bits(source);
        auto leftHalf = And32Bits(source >> 32);
        target = 0;
        for (auto roundIndex = 0u; roundIndex < 4; ++roundIndex)
        {
            RoundFunction(rightHalf, g_roundKeys[roundIndex]);
            rightHalf ^= leftHalf;
            std::swap(rightHalf, leftHalf);
        }

        std::swap(rightHalf, leftHalf);

        target += 0;
        target |= rightHalf;
        target |= leftHalf << 32;
    }

    void ResetKey()
    {
        for (auto& key : g_roundKeys)
        {
            key = 0;
        }
    }

    void Encrypt(const unsigned long long& source, unsigned long long& target, const unsigned long long* key = nullptr)
    {
        GenerateKeys(key);

        auto rightHalf = (source>>32) & 0xFFFFFFFF;
        auto leftHalf = source & 0xFFFFFFFF;
        ReverseBytes(4, rightHalf);
        ReverseBytes(4, leftHalf);
        std::swap(rightHalf, leftHalf);
        target = 0;
        for (auto roundIndex = 0u; roundIndex < 4; ++roundIndex)
        {
            auto tmp = 0ull;
            RoundFunction(rightHalf, g_roundKeysInt[roundIndex], tmp);
            tmp ^= leftHalf;
            rightHalf = leftHalf;
            leftHalf = tmp;
        }

        std::swap(rightHalf, leftHalf);

        target |= rightHalf<<32;
        target |= leftHalf;
        ReverseBytes(8,target);
    }

}


#pragma once

#ifdef QP_SOLVER_SPARSE
#include <Eigen/Sparse>
#endif

#include <Eigen/Dense>
#include <limits>
#include <vector>

#define QP_SOLVER_PRINTING

namespace qp_solver {

/** Quadratic Problem
 *  minimize        0.5 x' P x + q' x
 *  subject to      l <= A x <= u
 */
template <typename Scalar = double>
struct QuadraticProblem {
    using Vector = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
#ifdef QP_SOLVER_SPARSE
    using Matrix = Eigen::SparseMatrix<Scalar>;
    Eigen::Matrix<int, Eigen::Dynamic, 1> P_col_nnz;
    Eigen::Matrix<int, Eigen::Dynamic, 1> A_col_nnz;
#else
    using Matrix = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
#endif
    const Matrix *P;
    const Vector *q;
    const Matrix *A;
    const Vector *l;
    const Vector *u;
};

template <typename Scalar>
struct QPSolverSettings {
    Scalar rho = 1e-1;          /**< ADMM rho step, 0 < rho */
    Scalar sigma = 1e-6;        /**< ADMM sigma step, 0 < sigma, (small) */
    Scalar alpha = 1.0;         /**< ADMM overrelaxation parameter, 0 < alpha < 2,
                                     values in [1.5, 1.8] give good results (empirically) */
    Scalar eps_rel = 1e-3;      /**< Relative tolerance for termination, 0 < eps_rel */
    Scalar eps_abs = 1e-3;      /**< Absolute tolerance for termination, 0 < eps_abs */
    int max_iter = 1000;        /**< Maximal number of iteration, 0 < max_iter */
    int check_termination = 25; /**< Check termination after every Nth iteration, 0 (disabled) or 0
                                   < check_termination */
    bool warm_start = false;    /**< Warm start solver, reuses previous x,z,y */
    bool adaptive_rho = false;  /**< Adapt rho to optimal estimate */
    Scalar adaptive_rho_tolerance =
        5; /**< Minimal for rho update factor, 1 < adaptive_rho_tolerance */
    int adaptive_rho_interval = 25; /**< change rho every Nth iteration, 0 < adaptive_rho_interval,
                                         set equal to check_termination to save computation  */
    bool verbose = false;

#ifdef QP_SOLVER_PRINTING
    void print() const {
        printf("ADMM settings:\n");
        printf("  sigma %.2e\n", sigma);
        printf("  rho %.2e\n", rho);
        printf("  alpha %.2f\n", alpha);
        printf("  eps_rel %.1e\n", eps_rel);
        printf("  eps_abs %.1e\n", eps_abs);
        printf("  max_iter %d\n", max_iter);
        printf("  adaptive_rho %d\n", adaptive_rho);
        printf("  warm_start %d\n", warm_start);
    }
#endif
};

typedef enum { SOLVED, MAX_ITER_EXCEEDED, UNSOLVED, NUMERICAL_ISSUES, UNINITIALIZED } QPSolverStatus;

template <typename Scalar>
struct QPSolverInfo {
    QPSolverStatus status = UNINITIALIZED; /**< Solver status */
    int iter = 0;                          /**< Number of iterations */
    int rho_updates = 0;                   /**< Number of rho updates (factorizations) */
    Scalar rho_estimate = 0;               /**< Last rho estimate */
    Scalar res_prim = 0;                   /**< Primal residual */
    Scalar res_dual = 0;                   /**< Dual residual */

#ifdef QP_SOLVER_PRINTING
    void print() const {
        printf("ADMM info:\n");
        printf("  status ");
        switch (status) {
            case SOLVED:
                printf("SOLVED\n");
                break;
            case MAX_ITER_EXCEEDED:
                printf("MAX_ITER_EXCEEDED\n");
                break;
            case UNSOLVED:
                printf("UNSOLVED\n");
                break;
            case NUMERICAL_ISSUES:
                printf("NUMERICAL_ISSUES\n");
                break;
            default:
                printf("UNINITIALIZED\n");
        };
        printf("  iter %d\n", iter);
        printf("  rho_updates %d\n", rho_updates);
        printf("  rho_estimate %f\n", rho_estimate);
        printf("  res_prim %f\n", res_prim);
        printf("  res_dual %f\n", res_dual);
    }
#endif
};

/**
 *  minimize        0.5 x' P x + q' x
 *  subject to      l <= A x <= u
 *
 *  with:
 *    x element of R^n
 *    Ax element of R^m
 */
template <typename SCALAR>
class QPSolver {
   public:
    using Scalar = SCALAR;
    using QP = QuadraticProblem<Scalar>;
    using Vector = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
#ifdef QP_SOLVER_SPARSE
    using Matrix = Eigen::SparseMatrix<Scalar, Eigen::ColMajor>;
    using LinearSolver = Eigen::SimplicialLDLT<Matrix, Eigen::Lower>;
#else
    using Matrix = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using LinearSolver = Eigen::LDLT<Matrix, Eigen::Lower>;
#endif
    using Settings = QPSolverSettings<Scalar>;
    using Info = QPSolverInfo<Scalar>;

    enum { INEQUALITY_CONSTRAINT, EQUALITY_CONSTRAINT, LOOSE_BOUNDS } ConstraintType;

    static constexpr Scalar RHO_MIN = 1e-6;
    static constexpr Scalar RHO_MAX = 1e+6;
    static constexpr Scalar RHO_TOL = 1e-4;
    static constexpr Scalar RHO_EQ_FACTOR = 1e+3;
    static constexpr Scalar LOOSE_BOUNDS_THRESH = 1e+16;
    static constexpr Scalar DIV_BY_ZERO_REGUL = std::numeric_limits<Scalar>::epsilon();

    // enforce 16 byte alignment
    // https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    /** Constructor */
    QPSolver() = default;

    /** Setup solver for QP. */
    void setup(const QP &qp);

    /** Update solver for QP of same size as initial setup. */
    void update_qp(const QP &qp);

    /** Solve the QP. */
    void solve(const QP &qp);

    inline const Vector &primal_solution() const { return x; }
    inline Vector &primal_solution() { return x; }

    inline const Vector &dual_solution() const { return y; }
    inline Vector &dual_solution() { return y; }

    inline const Settings &settings() const { return settings_; }
    inline Settings &settings() { return settings_; }

    inline const Info &info() const { return info_; }
    inline Info &info() { return info_; }

    /* Public funcitions for unit testing */
    static void constr_type_init(const Vector& l, const Vector& u, Eigen::VectorXi &constr_type);

   private:
    /* Construct the KKT matrix of the form
     *
     * [[ P + sigma*I,        A' ],
     *  [ A,           -1/rho.*I ]]
     *
     * If LinearSolver_UpLo parameter is Eigen::Lower, then only the lower
     * triangular part is constructed to optimize memory.
     *
     * Note: For Eigen::ConjugateGradient it is advised to set Upper|Lower for
     *       best performance.
     */
    void construct_KKT_mat(const QP &qp);

    /** KKT matrix value update, assumes same sparsity pattern */
    void update_KKT_mat(const QP &qp);

    void update_KKT_rho();
    bool factorize_KKT();
    bool compute_KKT();
#ifdef QP_SOLVER_SPARSE
    void sparse_insert_at(Matrix &dst, int row, int col, const Matrix &src) const
#endif
        void form_KKT_rhs(const QP &qp, Vector &rhs);

    void box_projection(Vector &z, const Vector &l, const Vector &u);

    void constr_type_init(const QP &qp);
    void rho_vec_update(Scalar rho0);
    void update_state(const QP &qp);
    Scalar rho_estimate(const Scalar rho0, const QP &qp) const;
    Scalar eps_prim(const QP &qp) const;
    Scalar eps_dual(const QP &qp) const;
    Scalar residual_prim(const QP &qp) const;
    Scalar residual_dual(const QP &qp) const;

    bool termination_criteria(const QP &qp);

#ifdef QP_SOLVER_PRINTING
    void print_status(const QP &qp) const;
#endif

    size_t n;  //< number of variables
    size_t m;  //< number of constraints

    // Solver state variables
    int iter;
    Vector x;  //< primal variable, size n
    Vector z;  //< additional variable, size m
    Vector y;  //< dual variable, size m
    Vector x_tilde;
    Vector z_tilde;
    Vector z_prev;
    Vector rho_vec;
    Vector rho_inv_vec;
    Scalar rho;

    Vector rhs;
    Vector x_tilde_nu;

    // State
    Scalar res_prim;
    Scalar res_dual;
    Scalar max_Ax_z_norm_;
    Scalar max_Px_ATy_q_norm_;

    Eigen::VectorXi constr_type; /**< constraint type classification */

    Settings settings_;
    Info info_;

    Matrix kkt_mat;
    LinearSolver linear_solver;
};

extern template class QPSolver<double>;
extern template class QPSolver<float>;

}  // namespace qp_solver


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2009 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Authors: Katharina Kormann, Martin Kronbichler, Uppsala University,
 * 2009-2012, updated to MPI version with parallel vectors in 2016
 */

// 首先包括deal.II库中的必要文件。

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/timer.h>

#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/la_parallel_vector.h>
#include <deal.II/lac/precondition.h>

#include <deal.II/fe/fe_q.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>

#include <deal.II/multigrid/multigrid.h>
#include <deal.II/multigrid/mg_transfer_matrix_free.h>
#include <deal.II/multigrid/mg_tools.h>
#include <deal.II/multigrid/mg_coarse.h>
#include <deal.II/multigrid/mg_smoother.h>
#include <deal.II/multigrid/mg_matrix.h>

#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/vector_tools.h>

// 这包括有效实现无矩阵方法的数据结构，或者用MatrixFree类的更通用的有限元算子。

#include <deal.II/matrix_free/matrix_free.h>
#include <deal.II/matrix_free/operators.h>
#include <deal.II/matrix_free/fe_evaluation.h>

#include <iostream>
#include <fstream>

namespace Step37
{
  using namespace dealii;

// 为了提高效率，在无矩阵实现中进行的操作需要在编译时了解循环长度，这些长度是由有限元的度数给出的。因此，我们收集了两个模板参数的值，可以在代码中的一个地方改变。当然，我们可以把有限元的度数作为一个运行时的参数，通过编译所有可能的度数（比如，1到6之间）的计算核，并在运行时选择合适的核。在这里，我们只是选择二阶 $Q_2$ 元素，并选择维度3作为标准。

  const unsigned int degree_finite_element = 2;
  const unsigned int dimension             = 3;
// @sect3{Equation data}

// 我们为泊松问题定义了一个可变系数函数。它与 step-5 中的函数类似，但我们使用 $a(\mathbf x)=\frac{1}{0.05 + 2\|\bf x\|^2}$ 的形式，而不是不连续的形式。这只是为了证明这种实现的可能性，而不是在物理上有什么意义。我们定义系数的方式与早期教程程序中的函数相同。有一个新的函数，即有模板参数 @p value 的 @p number. 方法。
template <int dim>
  class Coefficient : public Function<dim>
  {
  public:
    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override;

    template <typename number>
    number value(const Point<dim, number> &p,
                 const unsigned int        component = 0) const;
  };

// 这就是上面提到的新函数。评估抽象类型的系数  @p number.  它可能只是一个普通的双数，但也可能是一个有点复杂的类型，我们称之为VectorizedArray。这种数据类型本质上是一个短的双数数组，正如在介绍中所讨论的那样，它可以容纳几个单元格的数据。例如，我们在这里评估的系数不是像通常那样在一个简单的点上，而是交给一个Point<dim,VectorizedArray<double>>点，在AVX的情况下，它实际上是四个点的集合。不要把VectorizedArray中的条目与点的不同坐标混淆。事实上，数据的布局是这样的： p[0] 返回一个VectorizedArray，它又包含了第一个点和第二个点的x坐标。你可以使用例如  p[0][j]  单独访问坐标，j=0,1,2,3，但建议尽可能在一个VectorizedArray上定义操作，以便利用矢量操作。

// 在函数的实现中，我们假设数字类型重载了基本的算术运算，所以我们只需照常写代码。然后，基类函数 @p value 是由带有双倍类型的模板函数计算出来的，以避免重复代码。

  template <int dim>
  template <typename number>
  number Coefficient<dim>::value(const Point<dim, number> &p,
                                 const unsigned int /*component*/) const
  {
    return 1. / (0.05 + 2. * p.square());
  }

  template <int dim>
  double Coefficient<dim>::value(const Point<dim> & p,
                                 const unsigned int component) const
  {
    return value<double>(p, component);
  }
// @sect3{Matrix-free implementation}

// 下面这个名为 LaplaceOperator 的类，实现了微分运算符。就所有的实用目的而言，它是一个矩阵，也就是说，你可以向它询问它的大小（成员函数  m(), n()  ），你可以将它应用于一个矢量（ vmult()  函数）。当然，与实数矩阵的区别在于，这个类实际上并不存储矩阵的elements，而只知道如何计算运算器应用于向量时的动作。

// 描述矩阵大小的基础结构，来自MatrixFree对象的初始化，以及通过vmult()和Tvmult()方法实现矩阵-向量乘积的各种接口，是由本类派生的 MatrixFreeOperator::Base 类提供的。这里定义的LaplaceOperator类只需要提供几个接口，即通过vmult()函数中使用的apply_add()方法来实现运算符的实际操作，以及计算底层矩阵对角线项的方法。我们需要对角线来定义多梯度平滑器。由于我们考虑的是一个具有可变系数的问题，我们进一步实现了一个可以填充系数值的方法。

// 注意文件 include/deal.II/matrix_free/operators.h 已经包含了通过类 MatrixFreeOperators::LaplaceOperator. 对拉普拉斯的实现。 出于教育目的，本教程程序中重新实现了该运算符，解释了其中的成分和概念。

// 这个程序利用了集成在deal.II中的有限元算子应用的数据缓存。这个数据缓存类被称为MatrixFree。它包含局部和全局自由度之间的映射信息（Jacobian）和索引关系。它还包含约束条件，如来自悬挂节点或迪里切特边界条件的约束。此外，它可以在所有单元上以%并行方式发出一个循环，确保只有不共享任何自由度的单元被处理（这使得循环在写入目标向量时是线程安全的）。与 @ref threads 模块中描述的WorkStream类相比，这是一个更先进的策略。当然，为了不破坏线程安全，我们在写进类全局结构时必须小心。

// 实现拉普拉斯算子的类有三个模板参数，一个是维度（正如许多deal.II类所携带的），一个是有限元的度数（我们需要通过FEEvaluation类来实现高效计算），还有一个是底层标量类型。我们希望对最终矩阵使用 double 数字（即双精度，64位浮点），但对多网格级矩阵使用浮点数（单精度，32位浮点数字）（因为那只是一个预处理程序，而浮点数的处理速度是两倍）。FEEvaluation类也需要一个模板参数，用于确定一维正交点的数量。在下面的代码中，我们把它硬编码为  fe_degree+1  。如果我们想独立于多项式程度来改变它，我们需要添加一个模板参数，就像在  MatrixFreeOperators::LaplaceOperator  类中做的那样。

// 顺便说一下，如果我们在同一个网格和自由度上实现了几个不同的操作（比如质量矩阵和拉普拉斯矩阵），我们将为每个操作者定义两个像现在这样的类（来自于 MatrixFreeOperators::Base 类），并让它们都引用一般问题类中的同一个MatrixFree数据缓存。通过 MatrixFreeOperators::Base 的接口要求我们只提供一组最小的函数。这个概念允许编写具有许多无矩阵操作的复杂应用代码。

//  @note  储存类型 VectorizedArray<number> 的值需要注意。在这里，我们使用deal.II表类，它准备以正确的对齐方式保存数据。然而，存储例如一个 std::vector<VectorizedArray<number> > 是不可能用矢量化的。数据与内存地址的边界需要一定的对齐（基本上，在AVX的情况下，一个32字节的VectorizedArray需要从一个能被32整除的内存地址开始）。表类（以及它所基于的AlignedVector类）确保这种对齐方式得到尊重，而 std::vector 一般不这样做，这可能会导致一些系统在奇怪的地方出现分段故障，或者其他系统的性能不理想。

  template <int dim, int fe_degree, typename number>
  class LaplaceOperator
    : public MatrixFreeOperators::
        Base<dim, LinearAlgebra::distributed::Vector<number>>
  {
  public:
    using value_type = number;

    LaplaceOperator();

    void clear() override;

    void evaluate_coefficient(const Coefficient<dim> &coefficient_function);

    virtual void compute_diagonal() override;

  private:
    virtual void apply_add(
      LinearAlgebra::distributed::Vector<number> &      dst,
      const LinearAlgebra::distributed::Vector<number> &src) const override;

    void
    local_apply(const MatrixFree<dim, number> &                   data,
                LinearAlgebra::distributed::Vector<number> &      dst,
                const LinearAlgebra::distributed::Vector<number> &src,
                const std::pair<unsigned int, unsigned int> &cell_range) const;

    void local_compute_diagonal(
      const MatrixFree<dim, number> &              data,
      LinearAlgebra::distributed::Vector<number> & dst,
      const unsigned int &                         dummy,
      const std::pair<unsigned int, unsigned int> &cell_range) const;

    Table<2, VectorizedArray<number>> coefficient;
  };

// 这是 @p LaplaceOperator 类的构造函数。它所做的就是调用基类 MatrixFreeOperators::Base, 的默认构造函数，而基类又是基于Subscriptor类的，它断言这个类在超出范围后不会被访问，比如在一个预处理程序中。

  template <int dim, int fe_degree, typename number>
  LaplaceOperator<dim, fe_degree, number>::LaplaceOperator()
    : MatrixFreeOperators::Base<dim,
                                LinearAlgebra::distributed::Vector<number>>()
  {}

  template <int dim, int fe_degree, typename number>
  void LaplaceOperator<dim, fe_degree, number>::clear()
  {
    coefficient.reinit(0, 0);
    MatrixFreeOperators::Base<dim, LinearAlgebra::distributed::Vector<number>>::
      clear();
  }

//  @sect4{Computation of coefficient}

// 为了初始化系数，我们直接赋予它上面定义的系数类，然后选择带有矢量数的方法 coefficient_function.value （编译器可以从点数据类型中推导出来）。下面将解释FEEvaluation类（及其模板参数）的使用。

  template <int dim, int fe_degree, typename number>
  void LaplaceOperator<dim, fe_degree, number>::evaluate_coefficient(
    const Coefficient<dim> &coefficient_function)
  {
    const unsigned int n_cells = this->data->n_cell_batches();
    FEEvaluation<dim, fe_degree, fe_degree + 1, 1, number> phi(*this->data);

    coefficient.reinit(n_cells, phi.n_q_points);
    for (unsigned int cell = 0; cell < n_cells; ++cell)
      {
        phi.reinit(cell);
        for (unsigned int q = 0; q < phi.n_q_points; ++q)
          coefficient(cell, q) =
            coefficient_function.value(phi.quadrature_point(q));
      }
  }

//  @sect4{Local evaluation of Laplace operator}

// 这里是这个类的主要功能，矩阵-向量乘积的评估（或者，一般来说，有限元算子评估）。这是在一个函数中完成的，该函数需要四个参数，MatrixFree对象，目标和源向量，以及要处理的单元格范围。MatrixFree类中的方法 cell_loop 将在内部用一些单元格范围来调用这个函数，这些单元格范围是通过检查哪些单元格可以同时工作来获得的，这样写操作就不会引起任何竞赛条件。请注意，循环中使用的单元格范围并不是直接指当前网格中的（活动）单元格数量，而是一个单元格批次的集合。 换句话说，"单元 "可能是一个错误的开始，因为FEEvaluation将几个单元的数据分组在一起。这意味着在正交点的循环中，我们实际上是将几个单元的正交点作为一个块来看待。这样做是为了实现更高的矢量化程度。 这种 "单元 "或 "单元批 "的数量存储在MatrixFree中，可以通过 MatrixFree::n_cell_batches(). 查询。与deal.II单元迭代器相比，在这个类中，所有的单元都被布置在一个普通的数组中，不直接知道水平或相邻关系，这使得通过无符号整数索引单元成为可能。

// 拉普拉斯运算符的实现非常简单。首先，我们需要创建一个对象FEEvaluation，它包含计算核，并有数据字段来存储临时结果（例如，在几个单元格集合的所有正交点上评估的梯度）。请注意，临时结果不会使用大量的内存，而且由于我们用元素顺序指定模板参数，数据被存储在堆栈中（没有昂贵的内存分配）。通常，只需要设置两个模板参数，维度作为第一个参数，有限元的度数作为第二个参数（这等于每个维度的自由度数减去FE_Q元素的一个）。然而，在这里，我们也希望能够使用浮点数来计算多网格预处理，这是最后一个（第五个）模板参数。因此，我们不能依赖默认的模板参数，因此必须填写第三和第四个字段。第三个参数指定每个方向的正交点的数量，其默认值等于元素的度数加1。第四个参数设置分量的数量（在PDEs系统中也可以评估矢量值的函数，但默认是标量元素），最后一个参数设置数字类型。

// 接下来，我们在给定的单元格范围内循环，然后继续进行实际的实现。

//
  告诉FEEvaluation对象我们要处理的（宏）单元。
  读入源向量的值（  @p read_dof_values),  包括约束的解析。这将存储 $u_\mathrm{cell}$ ，如介绍中所述。
  计算单元格梯度（有限元函数的评价）。由于FEEvaluation可以结合值计算和梯度计算，它使用一个统一的接口来处理0到2阶之间的各种导数。我们只想要梯度，不想要值，也不想要二阶导数，所以我们在梯度槽（第二槽）中将函数参数设置为真，而在值槽（第一槽）中设置为假。还有一个用于Hessian的第三槽，默认为假，所以不需要给它。请注意，FEEvaluation类在内部以一种有效的方式评估形状函数，一次只处理一个维度（如介绍中提到的使用形状函数和正交点的张量积形式）。与FEValues中使用的在所有局部自由度和正交点上循环的天真方法相比，在 $d$ 维度上，这给出了等于 $\mathcal O(d^2 (p+1)^{d+1})$ 的多项式度数 $p$ 的复杂度，并花费了 $\mathcal O(d (p+1)^{2d})$  。
  接下来是雅各布变换的应用，乘以变量系数和正交权重。FEEvaluation有一个访问函数 @p get_gradient ，可以应用Jacobian并返回实空间中的梯度。然后，我们只需要乘以（标量）系数，并让函数 @p submit_gradient 应用第二个雅各布式（用于测试函数）和正交权重及雅各布式行列式（JxW）。注意，提交的梯度存储在与 @p get_gradient. 中读取梯度的地方相同的数据字段中。因此，你需要确保在调用 @p submit_gradient 后不要再从同一正交点读取该特定正交点。一般来说，当 @p get_gradient 被多次使用时，复制其结果是个好主意。
  接下来是对所有测试函数的正交点进行求和，对应于实际积分步骤。对于拉普拉斯算子，我们只是乘以梯度，所以我们用各自的参数集调用积分函数。如果你有一个方程，同时用测试函数的值和梯度进行测试，那么两个模板参数都需要设置为真。先调用积分函数的值，再单独调用梯度，会导致错误的结果，因为第二次调用会在内部覆盖第一次调用的结果。请注意，积分步骤的二次导数没有函数参数。
  最终，介绍中提到的向量 $v_\mathrm{cell}$ 中的局部贡献需要被添加到结果向量中（并应用约束）。这是通过调用 @p distribute_local_to_global, 来完成的，该函数与AffineConstraints中的相应函数名称相同（只是我们现在将局部向量存储在FEEvaluation对象中，正如局部和全局自由度之间的指数一样）。

  template <int dim, int fe_degree, typename number>
  void LaplaceOperator<dim, fe_degree, number>::local_apply(
    const MatrixFree<dim, number> &                   data,
    LinearAlgebra::distributed::Vector<number> &      dst,
    const LinearAlgebra::distributed::Vector<number> &src,
    const std::pair<unsigned int, unsigned int> &     cell_range) const
  {
    FEEvaluation<dim, fe_degree, fe_degree + 1, 1, number> phi(data);

    for (unsigned int cell = cell_range.first; cell < cell_range.second; ++cell)
      {
        AssertDimension(coefficient.size(0), data.n_cell_batches());
        AssertDimension(coefficient.size(1), phi.n_q_points);

        phi.reinit(cell);
        phi.read_dof_values(src);
        phi.evaluate(EvaluationFlags::gradients);
        for (unsigned int q = 0; q < phi.n_q_points; ++q)
          phi.submit_gradient(coefficient(cell, q) * phi.get_gradient(q), q);
        phi.integrate(EvaluationFlags::gradients);
        phi.distribute_local_to_global(dst);
      }
  }

// 这个函数实现了对 Base::apply_add() 接口的所有单元的循环。这是用MatrixFree类的 @p cell_loop 来实现的，它接受这个类的operator()，参数为MatrixFree, OutVector, InVector, cell_range。当使用MPI并行化（但没有线程）时，如本教程程序中所做的，单元格循环对应于以下三行代码。

//
// @code
//  src.update_ghost_values();
//  local_apply(*this->data, dst, src, std::make_pair(0U,
//                                                    data.n_cell_batches()));
//  dst.compress(VectorOperation::add);
//  @endcode

// 这里，两个调用update_ghost_values()和compress()为MPI执行处理器边界上的数据交换，一次用于源向量，我们需要从远程处理器拥有的条目中读取，一次用于目的向量，我们已经积累了部分残余，需要添加到所有者处理器的相应条目中。然而， MatrixFree::cell_loop 不仅抽象出这两个调用，而且还进行了一些额外的优化。一方面，它将把update_ghost_values()和compress()的调用拆开，以允许通信和计算的重叠。然后用三个代表从0到 MatrixFree::n_cell_batches(). 的单元格范围的分区来调用local_apply函数。另一方面，cell_loop也支持线程并行，在这种情况下，单元格范围被分割成更小的块，并以一种先进的方式安排，避免了几个线程对同一个向量条目的访问。这一特性在  step-48  中有解释。

// 注意，在单元格循环之后，受约束的自由度需要再次被触及，以实现合理的vmult()操作。由于装配循环会自动解决约束问题（就像 AffineConstraints::distribute_local_to_global() 的调用一样），它不会计算对受约束自由度的任何贡献，而是将各自的条目留为零。这将表示一个矩阵的受限自由度的行和列都是空的。然而，像CG这样的迭代求解器只对非星形矩阵有效。最简单的方法是将矩阵中对应于受限自由度的子块设置为同一矩阵，在这种情况下，矩阵的应用只是将右侧向量的元素复制到左侧。幸运的是，vmult()的实现 MatrixFreeOperators::Base 在apply_add()函数之外自动为我们做了这个，所以我们不需要在这里采取进一步的行动。

// 当使用MatrixFree和FEEvaluation的组合与MPI并行时，有一个方面需要注意— 用于访问向量的索引。出于性能的考虑，MatrixFree和FEEvaluation被设计为在MPI本地索引空间中访问向量，当与多个处理器一起工作时也是如此。在本地索引空间工作意味着除了不可避免的间接寻址外，在向量访问发生的地方不需要进行索引转换。然而，本地索引空间是模糊的：虽然标准的惯例是用0和本地大小之间的索引访问向量的本地拥有的范围，但对于重影项的编号并不那么明确，而且有些随意。对于矩阵-向量乘积，只有出现在本地拥有的单元格上的指数（加上那些通过悬挂节点约束引用的指数）是必要的。然而，在deal.II中，我们经常将重影元素上的所有自由度设置为重影向量条目，称为 @ref GlossLocallyRelevantDof "术语表中描述的本地相关DoF"。在这种情况下，尽管指的是同一个全局索引，但在两个可能的重影集中，重影向量条目的MPI本地索引一般会有所不同。为了避免问题，FEEvaluation通过一个名为 LinearAlgebra::distributed::Vector::partitioners_are_compatible. 的检查来检查用于矩阵-向量乘积的向量分区是否确实与MatrixFree中的索引分区相匹配。 为了方便， MatrixFreeOperators::Base 类包括一个机制来使鬼魂集适合正确的布局。这发生在向量的重影区域，所以请记住，在调用vmult()方法后，目标和源向量的重影区域都可能被修改。这是合法的，因为分布式deal.II向量的ghost区域是一个可变的部分，并按需填充。在矩阵-向量乘积中使用的向量在进入vmult()函数时不能被重影，所以没有信息丢失。

  template <int dim, int fe_degree, typename number>
  void LaplaceOperator<dim, fe_degree, number>::apply_add(
    LinearAlgebra::distributed::Vector<number> &      dst,
    const LinearAlgebra::distributed::Vector<number> &src) const
  {
    this->data->cell_loop(&LaplaceOperator::local_apply, this, dst, src);
  }

// 下面的函数实现了算子对角线的计算。计算无矩阵算子评估的矩阵项，结果比评估算子更复杂。从根本上说，我们可以通过在all单位向量上应用算子来获得算子的矩阵表示。当然，这将是非常低效的，因为我们需要进行n运算符的评估来检索整个矩阵。此外，这种方法会完全忽视矩阵的稀疏性。然而，对于单个单元来说，这是一种方法，而且实际上效率并不低，因为单元内的所有自由度之间通常都存在着耦合。

// 我们首先将对角线向量初始化为正确的平行布局。这个向量被封装在基类 MatrixFreeOperators::Base. 中DiagonalMatrix类型的一个名为inverse_diagonal_entries的成员中，这个成员是一个共享指针，我们首先需要初始化它，然后获得代表矩阵中对角线条目的向量。至于实际的对角线计算，我们再次使用MatrixFree的cell_loop基础设施来调用一个名为local_compute_diagonal()的本地工作程序。由于我们只写进一个向量，而没有任何源向量，我们用一个<tt>unsigned int</tt>类型的假参数来代替源向量，以便与cell_loop接口确认。在循环之后，我们需要将受Dirichlet边界条件约束的向量条目设置为1（要么是MatrixFree内部AffineConstraints对象描述的边界上的条目，要么是自适应多网格中不同网格层次之间的索引）。这是通过函数 MatrixFreeOperators::Base::set_constrained_entries_to_one() 完成的，并与Base算子提供的矩阵-向量乘积中的设置相匹配。最后，我们需要反转对角线条目，这是基于Jacobi迭代的Chebyshev平滑器所要求的形式。在循环中，我们断言所有的条目都是非零的，因为它们应该从积分中获得正的贡献，或者被约束并被 @p set_constrained_entries_to_one() 以下的cell_loop处理。

  template <int dim, int fe_degree, typename number>
  void LaplaceOperator<dim, fe_degree, number>::compute_diagonal()
  {
    this->inverse_diagonal_entries.reset(
      new DiagonalMatrix<LinearAlgebra::distributed::Vector<number>>());
    LinearAlgebra::distributed::Vector<number> &inverse_diagonal =
      this->inverse_diagonal_entries->get_vector();
    this->data->initialize_dof_vector(inverse_diagonal);
    unsigned int dummy = 0;
    this->data->cell_loop(&LaplaceOperator::local_compute_diagonal,
                          this,
                          inverse_diagonal,
                          dummy);

    this->set_constrained_entries_to_one(inverse_diagonal);

    for (unsigned int i = 0; i < inverse_diagonal.locally_owned_size(); ++i)
      {
        Assert(inverse_diagonal.local_element(i) > 0.,
               ExcMessage("No diagonal entry in a positive definite operator "
                          "should be zero"));
        inverse_diagonal.local_element(i) =
          1. / inverse_diagonal.local_element(i);
      }
  }

// 在本地计算循环中，我们通过循环本地矩阵中的所有列来计算对角线，并将条目1放在i槽中，将条目0放在所有其他槽中，也就是说，我们一次在一个单位向量上应用单元格微分运算。调用 FEEvaluation::evaluate, 的内部部分是对正交点的循环， FEEvalution::integrate, 则与local_apply函数完全相同。之后，我们挑出本地结果的第i个条目，并将其放入一个临时存储器（因为我们在下一次循环迭代时覆盖了 FEEvaluation::get_dof_value() 后面数组中的所有条目）。最后，临时存储被写到目标向量中。注意我们是如何使用 FEEvaluation::get_dof_value() 和 FEEvaluation::submit_dof_value() 来读取和写入FEEvaluation用于积分的数据字段，并在另一方面写入全局向量的。

// 鉴于我们只对矩阵的对角线感兴趣，我们简单地扔掉了沿途计算过的本地矩阵的所有其他条目。虽然计算完整的单元格矩阵，然后扔掉除对角线以外的所有东西看起来很浪费，但是整合的效率很高，所以计算并没有花费太多时间。请注意，对于多项式度数来说，每个元素的算子评估的复杂度是 $\mathcal O((p+1)^{d+1})$ ，所以计算整个矩阵要花费我们 $\mathcal O((p+1)^{2d+1})$ 次操作，与用FEValues计算对角线的复杂度 $\mathcal O((p+1)^{2d})$ 相差不大。由于FEEvaluation也由于矢量化和其他优化而大大加快了速度，所以用这个函数计算对角线实际上是最快的（简单的）变量。(有可能用 $\mathcal O((p+1)^{d+1})$ 操作中的和分解技术来计算对角线，这涉及到特别适应的内核—但是由于这种内核只在特定的环境下有用，而对角线计算通常不在关键路径上，所以它们没有在deal.II中实现。)

// 注意在向量上调用distribution_local_to_global来将对角线条目累积到全局矩阵的代码有一些限制。对于带有悬空节点约束的操作者来说，在distribution_local_to_global的调用中，将一个受约束的DoF的积分贡献分配给其他几个条目，这里使用的向量接口并不完全计算对角线条目，而是将一些位于本地矩阵对角线上的贡献，最终在全局矩阵的非对角线位置堆积到对角线上。如Kormann (2016), section 5.3中所解释的，该结果在离散化精度上是正确的，但在数学上并不平等。在这个教程程序中，不会发生任何危害，因为对角线只用于没有悬空节点约束出现的多网格水平矩阵中。

  template <int dim, int fe_degree, typename number>
  void LaplaceOperator<dim, fe_degree, number>::local_compute_diagonal(
    const MatrixFree<dim, number> &             data,
    LinearAlgebra::distributed::Vector<number> &dst,
    const unsigned int &,
    const std::pair<unsigned int, unsigned int> &cell_range) const
  {
    FEEvaluation<dim, fe_degree, fe_degree + 1, 1, number> phi(data);

    AlignedVector<VectorizedArray<number>> diagonal(phi.dofs_per_cell);

    for (unsigned int cell = cell_range.first; cell < cell_range.second; ++cell)
      {
        AssertDimension(coefficient.size(0), data.n_cell_batches());
        AssertDimension(coefficient.size(1), phi.n_q_points);

        phi.reinit(cell);
        for (unsigned int i = 0; i < phi.dofs_per_cell; ++i)
          {
            for (unsigned int j = 0; j < phi.dofs_per_cell; ++j)
              phi.submit_dof_value(VectorizedArray<number>(), j);
            phi.submit_dof_value(make_vectorized_array<number>(1.), i);

            phi.evaluate(EvaluationFlags::gradients);
            for (unsigned int q = 0; q < phi.n_q_points; ++q)
              phi.submit_gradient(coefficient(cell, q) * phi.get_gradient(q),
                                  q);
            phi.integrate(EvaluationFlags::gradients);
            diagonal[i] = phi.get_dof_value(i);
          }
        for (unsigned int i = 0; i < phi.dofs_per_cell; ++i)
          phi.submit_dof_value(diagonal[i], i);
        phi.distribute_local_to_global(dst);
      }
  }

//  @sect3{LaplaceProblem class}

// 这个类是基于  step-16  中的一个。然而，我们用我们的无矩阵实现取代了SparseMatrix<double>类，这意味着我们也可以跳过稀疏性模式。请注意，我们定义LaplaceOperator类时，将有限元的度数作为模板参数（该值在文件的顶部定义），我们使用浮点数来表示多网格级矩阵。

// 该类还有一个成员变量，用来记录在我们真正去解决这个问题之前设置整个数据链的所有详细时间。此外，还有一个输出流（默认情况下是禁用的），可以用来输出各个设置操作的细节，而不是默认情况下只打印出的摘要。

// 由于这个程序被设计成与MPI一起使用，我们也提供了通常的 @p pcout 输出流，只打印MPI等级为0的处理器的信息。这个程序使用的网格可以是基于p4est的分布式三角图（在deal.II被配置为使用p4est的情况下），否则它就是一个只在没有MPI的情况下运行的串行网格。

  template <int dim>
  class LaplaceProblem
  {
  public:
    LaplaceProblem();
    void run();

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

#ifdef DEAL_II_WITH_P4EST
    parallel::distributed::Triangulation<dim> triangulation;
#else
    Triangulation<dim> triangulation;
#endif

    FE_Q<dim>       fe;
    DoFHandler<dim> dof_handler;

    MappingQ1<dim> mapping;

    AffineConstraints<double> constraints;
    using SystemMatrixType =
      LaplaceOperator<dim, degree_finite_element, double>;
    SystemMatrixType system_matrix;

    MGConstrainedDoFs mg_constrained_dofs;
    using LevelMatrixType = LaplaceOperator<dim, degree_finite_element, float>;
    MGLevelObject<LevelMatrixType> mg_matrices;

    LinearAlgebra::distributed::Vector<double> solution;
    LinearAlgebra::distributed::Vector<double> system_rhs;

    double             setup_time;
    ConditionalOStream pcout;
    ConditionalOStream time_details;
  };

// 当我们初始化有限元时，我们当然也要使用文件顶部指定的度数（否则，在某些时候会抛出一个异常，因为在模板化的LaplaceOperator类中定义的计算内核和MatrixFree读出的有限元信息将不匹配）。三角形的构造函数需要设置一个额外的标志，告诉网格要符合顶点上的2:1单元平衡，这对于几何多网格例程的收敛是必需的。对于分布式网格，我们还需要特别启用多网格的层次结构。

  template <int dim>
  LaplaceProblem<dim>::LaplaceProblem()
    :
#ifdef DEAL_II_WITH_P4EST
    triangulation(
      MPI_COMM_WORLD,
      Triangulation<dim>::limit_level_difference_at_vertices,
      parallel::distributed::Triangulation<dim>::construct_multigrid_hierarchy)
    ,
#else
    triangulation(Triangulation<dim>::limit_level_difference_at_vertices)
    ,
#endif
    fe(degree_finite_element)
    , dof_handler(triangulation)
    , setup_time(0.)
    , pcout(std::cout, Utilities::MPI::this_mpi_process(MPI_COMM_WORLD) == 0)
    ,

// LaplaceProblem类拥有一个额外的输出流，用于收集关于设置阶段的详细时间信息。这个流被称为time_details，默认情况下通过这里指定的 @p false 参数被禁用。对于详细的时间，去掉 @p false 参数可以打印出所有的细节。

    time_details(std::cout,
                 false && Utilities::MPI::this_mpi_process(MPI_COMM_WORLD) == 0)
  {}

//  @sect4{LaplaceProblem::setup_system}

// 设置阶段与 step-16 类似，由于LaplaceOperator类的存在而有相关的变化。首先要做的是设置DoFHandler，包括多网格层次的自由度，以及初始化悬挂节点的约束和同质二列条件。由于我们打算用MPI的%并行方式使用这个程序，我们需要确保约束条件能知道本地相关的自由度，否则在使用超过几亿个自由度的时候，存储会爆炸，见  step-40  。

// 一旦我们创建了多网格dof_handler和约束条件，我们就可以为全局矩阵算子以及多网格方案的每一层调用reinit函数。主要的操作是为问题设置  MatrixFree  实例。 LaplaceOperator 类的基类， MatrixFreeOperators::Base, 被初始化为一个指向MatrixFree对象的共享指针。这样，我们可以在这里简单地创建它，然后将它分别传递给系统矩阵和水平矩阵。为了设置MatrixFree，我们需要激活MatrixFree的AdditionalData字段中的更新标志，使其能够存储实空间中的正交点坐标（默认情况下，它只缓存梯度（反转置的雅各布）和JxW值的数据）。请注意，如果我们调用 reinit 函数而不指定级别（即给出  level = numbers::invalid_unsigned_int  ），MatrixFree 将在活动单元上构建一个循环。在本教程中，除了MPI之外，我们不使用线程，这就是为什么我们通过将 MatrixFree::AdditionalData::tasks_parallel_scheme 设置为 MatrixFree::AdditionalData::none. 来明确地禁用它 最后，系数被评估，向量被初始化，如上所述。

  template <int dim>
  void LaplaceProblem<dim>::setup_system()
  {
    Timer time;
    setup_time = 0;

    system_matrix.clear();
    mg_matrices.clear_elements();

    dof_handler.distribute_dofs(fe);
    dof_handler.distribute_mg_dofs();

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

    IndexSet locally_relevant_dofs;
    DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);

    constraints.clear();
    constraints.reinit(locally_relevant_dofs);
    DoFTools::make_hanging_node_constraints(dof_handler, constraints);
    VectorTools::interpolate_boundary_values(
      mapping, dof_handler, 0, Functions::ZeroFunction<dim>(), constraints);
    constraints.close();
    setup_time += time.wall_time();
    time_details << "Distribute DoFs & B.C.     (CPU/wall) " << time.cpu_time()
                 << "s/" << time.wall_time() << "s" << std::endl;
    time.restart();

    {
      typename MatrixFree<dim, double>::AdditionalData additional_data;
      additional_data.tasks_parallel_scheme =
        MatrixFree<dim, double>::AdditionalData::none;
      additional_data.mapping_update_flags =
        (update_gradients | update_JxW_values | update_quadrature_points);
      std::shared_ptr<MatrixFree<dim, double>> system_mf_storage(
        new MatrixFree<dim, double>());
      system_mf_storage->reinit(mapping,
                                dof_handler,
                                constraints,
                                QGauss<1>(fe.degree + 1),
                                additional_data);
      system_matrix.initialize(system_mf_storage);
    }

    system_matrix.evaluate_coefficient(Coefficient<dim>());

    system_matrix.initialize_dof_vector(solution);
    system_matrix.initialize_dof_vector(system_rhs);

    setup_time += time.wall_time();
    time_details << "Setup matrix-free system   (CPU/wall) " << time.cpu_time()
                 << "s/" << time.wall_time() << "s" << std::endl;
    time.restart();

// 接下来，初始化所有层次上的多网格方法的矩阵。数据结构MGConstrainedDoFs保留了受边界条件约束的指数信息，以及不同细化层次之间的边缘指数，如 step-16 教程程序中所述。然后，我们穿过网格的各个层次，在每个层次上构建约束和矩阵。这与原始网格上的系统矩阵的构造密切相关，只是在访问层级信息而不是活动单元的信息时，在命名上略有不同。

    const unsigned int nlevels = triangulation.n_global_levels();
    mg_matrices.resize(0, nlevels - 1);

    std::set<types::boundary_id> dirichlet_boundary;
    dirichlet_boundary.insert(0);
    mg_constrained_dofs.initialize(dof_handler);
    mg_constrained_dofs.make_zero_boundary_constraints(dof_handler,
                                                       dirichlet_boundary);

    for (unsigned int level = 0; level < nlevels; ++level)
      {
        IndexSet relevant_dofs;
        DoFTools::extract_locally_relevant_level_dofs(dof_handler,
                                                      level,
                                                      relevant_dofs);
        AffineConstraints<double> level_constraints;
        level_constraints.reinit(relevant_dofs);
        level_constraints.add_lines(
          mg_constrained_dofs.get_boundary_indices(level));
        level_constraints.close();

        typename MatrixFree<dim, float>::AdditionalData additional_data;
        additional_data.tasks_parallel_scheme =
          MatrixFree<dim, float>::AdditionalData::none;
        additional_data.mapping_update_flags =
          (update_gradients | update_JxW_values | update_quadrature_points);
        additional_data.mg_level = level;
        std::shared_ptr<MatrixFree<dim, float>> mg_mf_storage_level(
          new MatrixFree<dim, float>());
        mg_mf_storage_level->reinit(mapping,
                                    dof_handler,
                                    level_constraints,
                                    QGauss<1>(fe.degree + 1),
                                    additional_data);

        mg_matrices[level].initialize(mg_mf_storage_level,
                                      mg_constrained_dofs,
                                      level);
        mg_matrices[level].evaluate_coefficient(Coefficient<dim>());
      }
    setup_time += time.wall_time();
    time_details << "Setup matrix-free levels   (CPU/wall) " << time.cpu_time()
                 << "s/" << time.wall_time() << "s" << std::endl;
  }

//  @sect4{LaplaceProblem::assemble_rhs}

// 组装函数非常简单，因为我们所要做的就是组装右侧。多亏了FEEvaluation和所有缓存在MatrixFree类中的数据，我们从 MatrixFreeOperators::Base, 中查询，这可以在几行中完成。由于这个调用没有被包裹到 MatrixFree::cell_loop 中（这将是一个替代方案），我们一定不要忘记在装配结束时调用compress()，将右手边的所有贡献发送给各自自由度的所有者。

  template <int dim>
  void LaplaceProblem<dim>::assemble_rhs()
  {
    Timer time;

    system_rhs = 0;
    FEEvaluation<dim, degree_finite_element> phi(
      *system_matrix.get_matrix_free());
    for (unsigned int cell = 0;
         cell < system_matrix.get_matrix_free()->n_cell_batches();
         ++cell)
      {
        phi.reinit(cell);
        for (unsigned int q = 0; q < phi.n_q_points; ++q)
          phi.submit_value(make_vectorized_array<double>(1.0), q);
        phi.integrate(EvaluationFlags::values);
        phi.distribute_local_to_global(system_rhs);
      }
    system_rhs.compress(VectorOperation::add);

    setup_time += time.wall_time();
    time_details << "Assemble right hand side   (CPU/wall) " << time.cpu_time()
                 << "s/" << time.wall_time() << "s" << std::endl;
  }

//  @sect4{LaplaceProblem::solve}

// 解决的过程与  step-16  中类似。我们先从转移的设置开始。对于 LinearAlgebra::distributed::Vector, 来说，有一个非常快速的转移类，叫做MGTransferMatrixFree，它用FEEvaluation中同样的快速和因子化核在网格层之间进行插值。

  template <int dim>
  void LaplaceProblem<dim>::solve()
  {
    Timer                            time;
    MGTransferMatrixFree<dim, float> mg_transfer(mg_constrained_dofs);
    mg_transfer.build(dof_handler);
    setup_time += time.wall_time();
    time_details << "MG build transfer time     (CPU/wall) " << time.cpu_time()
                 << "s/" << time.wall_time() << "s\n";
    time.restart();

// 作为一个平滑器，本教程程序使用切比雪夫迭代，而不是 step-16 中的SOR。（SOR将很难实现，因为我们没有明确的矩阵元素，而且很难使其在%并行中有效工作）。 平滑器是用我们的水平矩阵和切比雪夫平滑器的强制性附加数据初始化的。我们在这里使用一个相对较高的度数（5），因为矩阵-向量乘积是比较便宜的。我们选择在平滑器中平滑出 $[1.2 \hat{\lambda}_{\max}/15,1.2 \hat{\lambda}_{\max}]$ 的范围，其中 $\hat{\lambda}_{\max}$ 是对最大特征值的估计（系数1.2在PreconditionChebyshev中应用）。为了计算该特征值，Chebyshev初始化执行了几步没有预处理的CG算法。由于最高的特征值通常是最容易找到的，而且一个粗略的估计就足够了，我们选择10次迭代。最后，我们还设置了切比雪夫方法中的内部预处理类型，这是一个雅可比迭代。这由DiagonalMatrix类来表示，该类得到了由我们的LaplaceOperator类提供的反对角线条目。

// 在第0层，我们以不同的方式初始化平滑器，因为我们想使用切比雪夫迭代作为求解器。PreconditionChebyshev允许用户切换到求解器模式，其中迭代次数在内部选择为正确值。在附加数据对象中，通过将多项式的度数选择为 @p numbers::invalid_unsigned_int. 来激活这一设置，然后算法将攻击粗级矩阵中最小和最大之间的所有特征值。切比雪夫平滑器的步数是这样选择的：切比雪夫收敛估计值保证将残差减少到变量 @p  smoothing_range中指定的数字。注意，对于求解来说， @p smoothing_range 是一个相对的公差，并且选择小于1，在这种情况下，我们选择三个数量级，而当只对选定的特征值进行平滑时，它是一个大于1的数字。

// 从计算的角度来看，只要粗粒度适中，Chebyshev迭代是一个非常有吸引力的粗粒度求解器。这是因为Chebyshev方法只执行矩阵-向量乘积和向量更新，这通常比其他迭代方法中涉及的内积更好地并行到有几万个核心的最大集群规模。前者只涉及到（粗）网格中邻居之间的局部通信，而后者则需要在所有处理器上进行全局通信。

    using SmootherType =
      PreconditionChebyshev<LevelMatrixType,
                            LinearAlgebra::distributed::Vector<float>>;
    mg::SmootherRelaxation<SmootherType,
                           LinearAlgebra::distributed::Vector<float>>
                                                         mg_smoother;
    MGLevelObject<typename SmootherType::AdditionalData> smoother_data;
    smoother_data.resize(0, triangulation.n_global_levels() - 1);
    for (unsigned int level = 0; level < triangulation.n_global_levels();
         ++level)
      {
        if (level > 0)
          {
            smoother_data[level].smoothing_range     = 15.;
            smoother_data[level].degree              = 5;
            smoother_data[level].eig_cg_n_iterations = 10;
          }
        else
          {
            smoother_data[0].smoothing_range = 1e-3;
            smoother_data[0].degree          = numbers::invalid_unsigned_int;
            smoother_data[0].eig_cg_n_iterations = mg_matrices[0].m();
          }
        mg_matrices[level].compute_diagonal();
        smoother_data[level].preconditioner =
          mg_matrices[level].get_matrix_diagonal_inverse();
      }
    mg_smoother.initialize(mg_matrices, smoother_data);

    MGCoarseGridApplySmoother<LinearAlgebra::distributed::Vector<float>>
      mg_coarse;
    mg_coarse.initialize(mg_smoother);

// 下一步是设置悬挂节点情况下所需的接口矩阵。deal.II中的自适应多网格实现了一种叫做局部平滑的方法。这意味着最细级别的平滑只覆盖固定（最细）网格级别所定义的网格的局部部分，而忽略了计算域中终端单元比该级别更粗的部分。随着该方法向更粗的级别发展，越来越多的全局网格将被覆盖。在某个更粗的层次上，整个网格将被覆盖。由于多网格方法中的所有层次矩阵都覆盖了网格中的单一层次，所以在层次矩阵上不会出现悬空节点。在多网格层之间的界面上，在平滑的同时设置同质Dirichlet边界条件。然而，当残差被转移到下一个更粗的层次时，需要考虑到多网格界面的耦合。这是由所谓的界面（或边缘）矩阵来完成的，它计算了被具有同质Dirichlet条件的层次矩阵所遗漏的残差部分。我们参考 @ref mg_paper "Janssen和Kanschat的多网格论文 "以了解更多细节。

// 对于这些接口矩阵的实现，已经有一个预定义的类 MatrixFreeOperators::MGInterfaceOperator ，它将例程 MatrixFreeOperators::Base::vmult_interface_down() 和 MatrixFreeOperators::Base::vmult_interface_up() 包装在一个带有 @p vmult()和 @p Tvmult() 操作（最初是为矩阵编写的，因此期待这些名字）的新类中。请注意，vmult_interface_down是在多网格V周期的限制阶段使用的，而vmult_interface_up是在延长阶段使用的。

// 一旦接口矩阵被创建，我们完全按照 step-16 的方法设置剩余的多网格预处理基础设施，以获得一个可以应用于矩阵的 @p preconditioner 对象。

    mg::Matrix<LinearAlgebra::distributed::Vector<float>> mg_matrix(
      mg_matrices);

    MGLevelObject<MatrixFreeOperators::MGInterfaceOperator<LevelMatrixType>>
      mg_interface_matrices;
    mg_interface_matrices.resize(0, triangulation.n_global_levels() - 1);
    for (unsigned int level = 0; level < triangulation.n_global_levels();
         ++level)
      mg_interface_matrices[level].initialize(mg_matrices[level]);
    mg::Matrix<LinearAlgebra::distributed::Vector<float>> mg_interface(
      mg_interface_matrices);

    Multigrid<LinearAlgebra::distributed::Vector<float>> mg(
      mg_matrix, mg_coarse, mg_transfer, mg_smoother, mg_smoother);
    mg.set_edge_matrices(mg_interface, mg_interface);

    PreconditionMG<dim,
                   LinearAlgebra::distributed::Vector<float>,
                   MGTransferMatrixFree<dim, float>>
      preconditioner(dof_handler, mg, mg_transfer);

// 多网格程序的设置非常简单，与  step-16  相比，在求解过程中看不出有什么不同。所有的魔法都隐藏在  LaplaceOperator::vmult  操作的实现背后。请注意，我们通过标准输出打印出求解时间和累积的设置时间，也就是说，在任何情况下，而设置操作的详细时间只在构造函数中的detail_times标志被改变的情况下打印。

    SolverControl solver_control(100, 1e-12 * system_rhs.l2_norm());
    SolverCG<LinearAlgebra::distributed::Vector<double>> cg(solver_control);
    setup_time += time.wall_time();
    time_details << "MG build smoother time     (CPU/wall) " << time.cpu_time()
                 << "s/" << time.wall_time() << "s\n";
    pcout << "Total setup time               (wall) " << setup_time << "s\n";

    time.reset();
    time.start();
    constraints.set_zero(solution);
    cg.solve(system_matrix, solution, system_rhs, preconditioner);

    constraints.distribute(solution);

    pcout << "Time solve (" << solver_control.last_step() << " iterations)"
          << (solver_control.last_step() < 10 ? "  " : " ") << "(CPU/wall) "
          << time.cpu_time() << "s/" << time.wall_time() << "s\n";
  }

//  @sect4{LaplaceProblem::output_results}

// 这里是数据输出，是  step-5  的简化版本。我们对细化过程中产生的每个网格使用标准的VTU（=压缩的VTK）输出。此外，我们还使用了一种针对速度而不是磁盘使用量进行优化的压缩算法。默认设置（针对磁盘使用进行优化）使得保存输出的时间是运行线性求解器的4倍，而将 DataOutBase::VtkFlags::compression_level 设置为 DataOutBase::VtkFlags::best_speed 则将其降低到只有线性求解的四分之一的时间。

// 当网格过大时，我们禁用输出。这个程序的一个变种已经在几十万个MPI行列上运行，网格单元多达1000亿个，经典的可视化工具无法直接访问。

  template <int dim>
  void LaplaceProblem<dim>::output_results(const unsigned int cycle) const
  {
    Timer time;
    if (triangulation.n_global_active_cells() > 1000000)
      return;

    DataOut<dim> data_out;

    solution.update_ghost_values();
    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution, "solution");
    data_out.build_patches(mapping);

    DataOutBase::VtkFlags flags;
    flags.compression_level = DataOutBase::VtkFlags::best_speed;
    data_out.set_flags(flags);
    data_out.write_vtu_with_pvtu_record(
      "./", "solution", cycle, MPI_COMM_WORLD, 3);

    time_details << "Time write output          (CPU/wall) " << time.cpu_time()
                 << "s/" << time.wall_time() << "s\n";
  }

//  @sect4{LaplaceProblem::run}

// 运行该程序的函数与  step-16  中的函数非常相似。与2D相比，我们在3D中做了很少的细化步骤，但仅此而已。

// 在运行程序之前，我们先输出一些关于检测到的矢量化水平的信息，正如在介绍中所讨论的那样。

  template <int dim>
  void LaplaceProblem<dim>::run()
  {
    {
      const unsigned int n_vect_doubles = VectorizedArray<double>::size();
      const unsigned int n_vect_bits    = 8 * sizeof(double) * n_vect_doubles;

      pcout << "Vectorization over " << n_vect_doubles
            << " doubles = " << n_vect_bits << " bits ("
            << Utilities::System::get_current_vectorization_level() << ")"
            << std::endl;
    }

    for (unsigned int cycle = 0; cycle < 9 - dim; ++cycle)
      {
        pcout << "Cycle " << cycle << std::endl;

        if (cycle == 0)
          {
            GridGenerator::hyper_cube(triangulation, 0., 1.);
            triangulation.refine_global(3 - dim);
          }
        triangulation.refine_global(1);
        setup_system();
        assemble_rhs();
        solve();
        output_results(cycle);
        pcout << std::endl;
      };
  }
} // namespace Step37

//  @sect3{The main function}

// 除了我们根据 step-40 设置了MPI框架外，主函数中没有任何意外。

int main(int argc, char *argv[])
{
  try
    {
      using namespace Step37;

      Utilities::MPI::MPI_InitFinalize mpi_init(argc, argv, 1);

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

  return 0;
}


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <BayesFilters/WhiteNoiseAcceleration.h>
#include <BayesFilters/utils.h>

#include <cmath>
#include <utility>

#include <Eigen/Cholesky>

using namespace bfl;
using namespace Eigen;

struct WhiteNoiseAcceleration::ImplData
{
    ImplData
    (
        const Dim dim,
        const double sampling_interval,
        const double tilde_q,
        const unsigned int seed
    ) :
        T_(sampling_interval),
        tilde_q_(tilde_q),
        seed_(seed),
        generator_(std::mt19937_64(seed_)),
        distribution_(std::normal_distribution<double>(0.0, 1.0)),
        gauss_rnd_sample_([&] { return (distribution_) (generator_); })
    {
        double q11 = 1.0 / 3.0 * std::pow(T_, 3.0);
        double q2 = 1.0 / 2.0 * std::pow(T_, 2.0);

        Matrix2d F;
        F << 1.0, T_,
             0.0, 1.0;

        Matrix2d Q;
        Q << q11, q2,
              q2, T_;

        switch (dim)
        {
            case Dim::OneD:
            {
                F_ = F;

                Q_ = Q;

                break;
            }

            case Dim::TwoD:
            {
                F_ = MatrixXd(4, 4);
                F_ << F, Matrix2d::Zero(),
                      Matrix2d::Zero(), F;

                Q_ = MatrixXd(4, 4);
                Q_ << Q, Matrix2d::Zero(),
                      Matrix2d::Zero(), Q;

                break;
            }

            case Dim::ThreeD:
            {
                F_ = MatrixXd(6, 6);
                F_ << F, Matrix2d::Zero(), Matrix2d::Zero(),
                      Matrix2d::Zero(), F, Matrix2d::Zero(),
                      Matrix2d::Zero(), Matrix2d::Zero(), F;

                Q_ = MatrixXd(6, 6);
                Q_ << Q, Matrix2d::Zero(), Matrix2d::Zero(),
                      Matrix2d::Zero(), Q, Matrix2d::Zero(),
                      Matrix2d::Zero(), Matrix2d::Zero(), Q;

                break;
            }
        }

        Q_ *= tilde_q_;

        LDLT<MatrixXd> chol_ldlt(Q_);
        sqrt_Q_ = (chol_ldlt.transpositionsP() * MatrixXd::Identity(Q_.rows(), Q_.cols())).transpose() * chol_ldlt.matrixL() * chol_ldlt.vectorD().real().cwiseSqrt().asDiagonal();
    }

    /**
     * Sampling interval in [time].
     */
    double T_;

    /**
     * Power spectral density [length]^2/[time]^3.
     */
    double tilde_q_;

    /**
     * State transition matrix.
     */
    Eigen::MatrixXd F_;

    /**
     * Convariance matrix of the additive white noise of the state model.
     */
    Eigen::MatrixXd Q_;

    /**
     * Square root matrix of R_.
     */
    Eigen::MatrixXd sqrt_Q_;

    /**
     * Seed of the random number generator.
     */
    unsigned int seed_;

    /**
     * Random number generator.
     */
    std::mt19937_64 generator_;

    /**
     * Normal distribution for random number generation functions.
     */
    std::normal_distribution<double> distribution_;

    /**
     * Random number generator function from a Normal distribution.
     * A call to `gauss_rnd_sample_()` returns a double-precision floating point random number.
     */
    std::function<double()> gauss_rnd_sample_;
};

WhiteNoiseAcceleration::WhiteNoiseAcceleration
(
    const Dim dim,
    const double sampling_interval,
    const double tilde_q
) noexcept :
    WhiteNoiseAcceleration(dim, sampling_interval, tilde_q, 1)
{ }

WhiteNoiseAcceleration::WhiteNoiseAcceleration
(
    const Dim dim,
    const double sampling_interval,
    const double tilde_q,
    const unsigned int seed
) noexcept :
    pimpl_(utils::make_unique<ImplData>(dim, sampling_interval, tilde_q, seed))
{ }

WhiteNoiseAcceleration::WhiteNoiseAcceleration(WhiteNoiseAcceleration&& state_model) noexcept = default;

WhiteNoiseAcceleration& WhiteNoiseAcceleration::operator=(WhiteNoiseAcceleration&& state_model) noexcept = default;

WhiteNoiseAcceleration::~WhiteNoiseAcceleration() noexcept = default;

bool WhiteNoiseAcceleration::setProperty(const std::string& property)
{
    return false;
}

std::pair<std::size_t, std::size_t> WhiteNoiseAcceleration::getOutputSize() const
{
    return std::make_pair(4, 0);
}

MatrixXd WhiteNoiseAcceleration::getNoiseSample(const std::size_t num)
{
    MatrixXd rand_vectors(4, num);
    for (int i = 0; i < rand_vectors.size(); i++)
        *(rand_vectors.data() + i) = pimpl_->gauss_rnd_sample_();

    return pimpl_->sqrt_Q_ * rand_vectors;
}

MatrixXd WhiteNoiseAcceleration::getNoiseCovarianceMatrix()
{
    return pimpl_->Q_;
}

MatrixXd WhiteNoiseAcceleration::getStateTransitionMatrix()
{
    return pimpl_->F_;
}

VectorXd WhiteNoiseAcceleration::getTransitionProbability(const Ref<const MatrixXd>& prev_states, const Ref<const MatrixXd>& cur_states)
{
    return utils::multivariate_gaussian_density(prev_states, prev_states.col(0), pimpl_->Q_);
}


#include <iostream>
#include <fstream>
#include <boost/program_options.hpp>
#include <random>
#include <iomanip>
#include <chrono>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_blocked.hpp>
#include "matrix.h"

namespace po = boost::program_options;
using namespace std;
std::uniform_real_distribution<double> urd;
std::default_random_engine re;

Tmat RandomMatrix(int m, int n) {
  Tmat mat = newMat(m, n);
  for (int i = 0; i < m; i += 1) {
    for (int j = 0; j < n; j += 1) {
      mat[j][i] = urd(re);
    }
  }
  return mat;
}
void prikaz1(Tmat &mat) {
  for (auto &vrstica : mat) {
    std::cout << "|";
    for (auto &element : vrstica) {
      std::cout << std::setw(10) << element;
    }
    std::cout << "|" << std::endl;
  }
  std::cout << std::endl;
}
boost::numeric::ublas::matrix<double> TmatToBlas(Tmat mat) {
  int m1 = mat.size();
  int n1 = mat[0].size();
  boost::numeric::ublas::matrix<double> bmat(m1, n1);
  for (int i = 0; i < m1; i += 1) {
    for (int j = 0; j < n1; j += 1) {
      bmat(i, j) = mat[i][j];
    }
  }
  return bmat;
}

boost::numeric::ublas::matrix<double> MultiplicationBlas(boost::numeric::ublas::matrix<double> M1,
                                                         boost::numeric::ublas::matrix<double> M2) {
  return boost::numeric::ublas::block_prod<boost::numeric::ublas::matrix<double>, 32>(M1, M2);
}

enum Method {
  CLASSIC, CLASSIC_T, RECURSIVE, RECURSIVE_T, SUBCUBIC, BLAS //STRASSEN, BLAS
};
static map<string, Method> methodMap{
    {"classic",              CLASSIC},
    {"classic_transposed",   CLASSIC_T},
    {"recursive",            RECURSIVE},
    {"recursive_transposed", RECURSIVE_T},
    {"subcubic",             SUBCUBIC},
    //{"strassen",             STRASSEN},
    {"blas",                 BLAS}
};

int main(int ac, const char **av) {
  int a, b, c;
  int repeat;
  int seed = 2; //totaly random
  int max_time;
  Method method;
  try {
    po::options_description desc("Allowed options");
    desc.add_options()
        ("help", "Display this message")
        ("method,m", po::value<string>()->default_value("subcubic"), "Method to benchmark \n"
            "Available methods:\n"
            "  classic\n"
            "  classic_transposed\n"
            "  recursive\n"
            "  recursive_transposed\n"
            "  subcubic \n"
            //"  strassen \n"
            "  blas \n"
        )
        ("a,a", po::value<int>()->default_value(512), "A in (A,B)x(B,C) mutiplication  \n ")
        ("b,b", po::value<int>()->default_value(512), "B in (A,B)x(B,C) mutiplication  \n ")
        ("c,c", po::value<int>()->default_value(512), "C in (A,B)x(B,C) mutiplication  \n ")
        ("repeat,r", po::value<int>()->default_value(1), "Number of tests to run")
        ("time,t", po::value<int>()->default_value(0), "Repeat test until arg ms elapsed, overrides --repeat \n ");

    po::variables_map vm;
    po::store(po::parse_command_line(ac, av, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
      cout << desc << "\n";
      return 1;
    }

    if (vm.count("method")) {
      string m = vm["method"].as<string>();
      if (methodMap.count(m) > 0) {
        //cout << "testing with method " << m << '\n';
        method = methodMap[m];
      } else {
        cout << m << " is not a valid method, use --help for a list of available methods.";
        return 1;
      }
    } else {
      cout << "method was not set!\n";
      return 1;
    }

    if (vm.count("a")) {
      a = vm["a"].as<int>();
      //cout << "a = " << a << '\n';

    } else {
      cout << "a was not set!\n";
      return 1;
    }
    if (vm.count("b")) {
      b = vm["b"].as<int>();
      //cout << "b = " << b << '\n';

    } else {
      cout << "b was not set!\n";
      return 1;
    }
    if (vm.count("c")) {
      c = vm["c"].as<int>();
      // cout << "c = " << c << '\n';

    } else {
      cout << "c was not set!\n";
      return 1;
    }

    if (vm.count("time")) {
      max_time = vm["time"].as<int>();
    }

    repeat = vm["repeat"].as<int>();
    if (max_time == 0) {
      //cout << "Number of test: " << repeat << endl;
    } else {
      //cout << "Testing for " << max_time << " ms" << endl;
    }

  } catch (std::exception &e) {
    cerr << e.what();
  }

  //Execute the tests
  urd = std::uniform_real_distribution<double>(0, 1);
  re = std::default_random_engine(seed);
  Tmat m1 = RandomMatrix(b, a);
  Tmat m2 = RandomMatrix(c, b);
  boost::numeric::ublas::matrix<double> bm1, bm2;

  std::function<Tmat(Tmat &, Tmat &)> f;
  auto blas_f = MultiplicationBlas;

  switch (method) {
    case CLASSIC:f = MultiplicationClassic;
      break;
    case CLASSIC_T:f = MultiplicationClassicTransposed;
      break;
    case RECURSIVE:f = MultiplicationRecursive;
      break;
    case RECURSIVE_T:f = MultiplicationRecursiveTransposed;
      break;
    case SUBCUBIC:f = MultiplicationSubcubic;
      break;
    //case STRASSEN: f = strassen_mul;
    //  break;
    case BLAS :bm1 = TmatToBlas(m1);
      bm2 = TmatToBlas(m2);
  }
  if (max_time == 1) {
    //placeholder for testing
    int n = 512;
    Tmat mat1 = newMat(n, n);
    Tmat mat2 = newMat(n, n);
    double konst = 100000000.000002;
    for (int i = 0; i < n; i += 1) {
      for (int j = 0; j < n; j += 1) {
        mat1[i][j] = konst;
        mat2[i][j] = konst;
      }
    }
    Tmat mat3 = f(mat1, mat2);
    //prikaz1(mat3);
    cout << setprecision(24);
    cout << konst * konst * n << endl << mat3[0][0] << endl;
    cout << mat3[n / 2][n / 2] - konst * konst * n << endl;
  } else {

    if (max_time > 0) {
      int count = 0;
      std::chrono::time_point<std::chrono::steady_clock> time_start, time_end;
      auto time_total = std::chrono::milliseconds{0};
      while (true) {
        time_start = std::chrono::steady_clock::now();
        if (method == BLAS) {
          auto bm3 = blas_f(bm1, bm2);
        } else {
          Tmat m3 = f(m1, m2);
        }
        time_end = std::chrono::steady_clock::now();
        auto time_of_test = std::chrono::duration_cast<std::chrono::milliseconds>(time_end - time_start);
//cout << time_of_test.count() << endl;
        time_total +=
            time_of_test;
        count++;
        if (time_total.
            count()
            > max_time || count > 10000) {
//cout << "Total time:" << time_total.count() << "ms" << endl;
//cout << "Iteration time:" << time_total.count() / count << "ms" << endl;
          cout << time_total.
              count()
              / count <<
               endl;
          break;
        }
      }
    } else {

      std::chrono::time_point<std::chrono::steady_clock> time_start, time_end;
      time_start = std::chrono::steady_clock::now();
      if (method == BLAS) {
        for (
            int i = 0;
            i < repeat;
            ++i) {
          auto bm3 = blas_f(bm1, bm2);
        }
      } else {
        for (
            int i = 0;
            i < repeat;
            ++i) {
          Tmat m3 = f(m1, m2);
        }
      }

      time_end = std::chrono::steady_clock::now();
      auto time_total = std::chrono::duration_cast<std::chrono::milliseconds>(time_end - time_start);
      std::cout << time_total.count() / repeat << endl;
    }
  }
}