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

/*
  Copyright (C) 2019 Klaus Spanderen

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

#include "fdsabr.hpp"
#include "utilities.hpp"
#include <ql/functional.hpp>
#include <ql/instruments/vanillaoption.hpp>
#include <ql/math/comparison.hpp>
#include <ql/math/randomnumbers/rngtraits.hpp>
#include <ql/math/randomnumbers/sobolbrownianbridgersg.hpp>
#include <ql/math/richardsonextrapolation.hpp>
#include <ql/math/statistics/generalstatistics.hpp>
#include <ql/methods/finitedifferences/utilities/cevrndcalculator.hpp>
#include <ql/pricingengines/vanilla/analyticcevengine.hpp>
#include <ql/pricingengines/vanilla/fdsabrvanillaengine.hpp>
#include <ql/processes/blackscholesprocess.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/volatility/sabr.hpp>
#include <boost/make_shared.hpp>
#include <utility>

using namespace QuantLib;
using boost::unit_test_framework::test_suite;

namespace {
    class SabrMonteCarloPricer {
      public:
        SabrMonteCarloPricer(Real f0,
                             Time maturity,
                             ext::shared_ptr<Payoff> payoff,
                             Real alpha,
                             Real beta,
                             Real nu,
                             Real rho)
        : f0_(f0), maturity_(maturity), payoff_(std::move(payoff)), alpha_(alpha), beta_(beta),
          nu_(nu), rho_(rho) {}

        Real operator()(Real dt) const {
            const Size nSims = 64*1024;

            const Real timeStepsPerYear = 1./dt;
            const Size timeSteps = Size(maturity_*timeStepsPerYear+1e-8);

            const Real sqrtDt = std::sqrt(dt);
            const Real w = std::sqrt(1.0-rho_*rho_);

            const Real logAlpha = std::log(alpha_);

            SobolBrownianBridgeRsg rsg(2, timeSteps, SobolBrownianGenerator::Diagonal, 12345U);

            GeneralStatistics stats;

            for (Size i=0; i < nSims; ++i) {
                Real f = f0_;
                Real a = logAlpha;

                const std::vector<Real> n = rsg.nextSequence().value;

                for (Size j=0; j < timeSteps && f > 0.0; ++j) {

                    const Real r1 = n[j];
                    const Real r2 = rho_*r1 + n[j+timeSteps]*w;

                    //Sample CEV distribution: accurate but slow
                    //
                    //const CEVRNDCalculator calc(f, std::exp(a), beta_);
                    //const Real u = CumulativeNormalDistribution()(r1);
                    //f = calc.invcdf(u, dt);

                    // simple Euler method
                    f += std::exp(a)*std::pow(f, beta_)*r1*sqrtDt;
                    a += - 0.5*nu_*nu_*dt + nu_*r2*sqrtDt;
                }
                f = std::max(0.0, f);
                stats.add((*payoff_)(f));
            }

            return stats.mean();
        }

      private:
        const Real f0_;
        const Time maturity_;
        const ext::shared_ptr<Payoff> payoff_;
        const Real alpha_, beta_, nu_, rho_;
    };

}

void FdSabrTest::testFdmSabrOp() {
    BOOST_TEST_MESSAGE("Testing FDM SABR operator...");

    SavedSettings backup;

    const Date today = Date(22, February, 2018);
    const DayCounter dc = Actual365Fixed();
    Settings::instance().evaluationDate() = today;

    const Date maturityDate = today + Period(2, Years);
    const Time maturityTime = dc.yearFraction(today, maturityDate);

    const Real strike = 1.5;

    const ext::shared_ptr<Exercise> exercise =
        ext::make_shared<EuropeanExercise>(maturityDate);

    const ext::shared_ptr<PlainVanillaPayoff> putPayoff =
        ext::make_shared<PlainVanillaPayoff>(Option::Put, strike);
    const ext::shared_ptr<PlainVanillaPayoff> callPayoff =
        ext::make_shared<PlainVanillaPayoff>(Option::Call, strike);

    VanillaOption optionPut(putPayoff, exercise);
    VanillaOption optionCall(callPayoff, exercise);

    const Handle<YieldTermStructure> rTS =
        Handle<YieldTermStructure>(flatRate(today, 0.0, dc));

    const Real f0    = 1.0;
    const Real alpha = 0.35;
    const Real nu    = 1.0;
    const Real rho   = 0.25;

    const Real betas[] = { 0.25, 0.6 };

    const ext::shared_ptr<GeneralizedBlackScholesProcess> bsProcess =
        ext::make_shared<GeneralizedBlackScholesProcess>(
            Handle<Quote>(ext::make_shared<SimpleQuote>(f0)),
            rTS, rTS, Handle<BlackVolTermStructure>(flatVol(0.2, dc)));

    for (double beta : betas) {

        const ext::shared_ptr<PricingEngine> pdeEngine =
            ext::make_shared<FdSabrVanillaEngine>(f0, alpha, beta, nu, rho, rTS, 100, 400, 100);

        optionPut.setPricingEngine(pdeEngine);
        const Real pdePut = optionPut.NPV();

        // check put/call parity
        optionCall.setPricingEngine(pdeEngine);
        const Real pdeCall = optionCall.NPV();

        const Real pdeFwd = pdeCall - pdePut;

        const Real parityDiff = std::fabs(pdeFwd - (f0 - strike));
        const Real parityTol = 1e-4;
        if (parityDiff > parityTol) {
            BOOST_ERROR(
                "failed to validate the call/put parity"
                << "\n    beta           : " << beta
                << "\n    strike         : " << strike
                << "\n    fwd (call/put) : " << pdeFwd
                << "\n    fwd (f0-strike): " << f0-strike
                << "\n    diff           : " << parityDiff
                << "\n    tol            : " << parityTol);
        }

        const Real putPdeImplVol =
            optionPut.impliedVolatility(optionPut.NPV(), bsProcess, 1e-6);

        const ext::function<Real(Real)> mcSabr(
            SabrMonteCarloPricer(f0, maturityTime, putPayoff,
                                 alpha, beta, nu, rho));

        const Real mcNPV = RichardsonExtrapolation(
            mcSabr, 1/4.0)(4.0, 2.0);

        const Real putMcImplVol =
            optionPut.impliedVolatility(mcNPV, bsProcess, 1e-6);

        const Real volDiff = std::fabs(putPdeImplVol - putMcImplVol);

        const Real volTol = 5e-3;
        if (volDiff > volTol) {
            BOOST_ERROR(
                "failed to validate PDE against MC implied volatility"
                << "\n    beta         : " << beta
                << "\n    strike       : " << strike
                << "\n    PDE impl vol : " << putPdeImplVol
                << "\n    MC  impl vol : " << putMcImplVol
                << "\n    diff         : " << volDiff
                << "\n    tol          : " << volTol);
        }
    }
}

void FdSabrTest::testFdmSabrCevPricing() {
    BOOST_TEST_MESSAGE("Testing FDM CEV pricing with trivial SABR model...");

    SavedSettings backup;

    const Date today = Date(3, January, 2019);
    const DayCounter dc = Actual365Fixed();
    Settings::instance().evaluationDate() = today;

    const Date maturityDate = today + Period(12, Months);

    const Real betas[]   = { 0.1, 0.9 };
    const Real strikes[] = { 0.9, 1.5 };

    const Real f0    = 1.2;
    const Real alpha = 0.35;
    const Real nu    = 1e-3;
    const Real rho   = 0.25;

    const Handle<YieldTermStructure> rTS = Handle<YieldTermStructure>(
        flatRate(today, 0.05, dc));

    const ext::shared_ptr<Exercise> exercise =
        ext::make_shared<EuropeanExercise>(maturityDate);

    const Option::Type optionTypes[] = {Option::Put, Option::Call};

    const Real tol = 5e-5;

    for (auto optionType : optionTypes) {
        for (double strike : strikes) {
            const ext::shared_ptr<PlainVanillaPayoff> payoff =
                ext::make_shared<PlainVanillaPayoff>(optionType, strike);

            VanillaOption option(payoff, exercise);

            for (double beta : betas) {
                option.setPricingEngine(ext::make_shared<FdSabrVanillaEngine>(
                    f0, alpha, beta, nu, rho, rTS, 100, 400, 3));

                const Real calculated = option.NPV();

                option.setPricingEngine(ext::make_shared<AnalyticCEVEngine>(
                    f0, alpha, beta, rTS));

                const Real expected = option.NPV();

                if (std::fabs(expected-calculated) > tol) {
                    BOOST_ERROR(
                        "failed to calculate vanilla CEV option prices"
                        << "\n    beta            : " << beta
                        << "\n    strike          : " << strike
                        << "\n    option type     : "
                        << ((payoff->optionType() == Option::Call) ? "Call" : "Put")
                        << "\n    analytic npv    : " << expected
                        << "\n    pde npv         : " << calculated
                        << "\n    npv difference  : "
                        << std::fabs(expected - calculated)
                        << "\n    tolerance       : " << tol);
                }
            }
        }
    }
}

void FdSabrTest::testFdmSabrVsVolApproximation() {
    BOOST_TEST_MESSAGE("Testing FDM SABR vs approximations...");

    SavedSettings backup;

    const Date today = Date(8, January, 2019);
    const DayCounter dc = Actual365Fixed();
    Settings::instance().evaluationDate() = today;

    const Date maturityDate = today + Period(6, Months);
    const Time maturityTime = dc.yearFraction(today, maturityDate);

    const Handle<YieldTermStructure> rTS = Handle<YieldTermStructure>(
        flatRate(today, 0.05, dc));

    const Real f0 = 100;

    const ext::shared_ptr<GeneralizedBlackScholesProcess> bsProcess =
        ext::make_shared<GeneralizedBlackScholesProcess>(
            Handle<Quote>(ext::make_shared<SimpleQuote>(f0)),
            rTS, rTS, Handle<BlackVolTermStructure>(flatVol(0.2, dc)));

    const Real alpha = 0.35;
    const Real beta  = 0.85;
    const Real nu    = 0.75;
    const Real rho   = 0.85;

    const Real strikes[] = { 90, 100, 110};
    const Option::Type optionTypes[] = {Option::Put, Option::Call};

    const Real tol = 2.5e-3;
    for (auto optionType : optionTypes) {
        for (double strike : strikes) {
            VanillaOption option(ext::make_shared<PlainVanillaPayoff>(optionType, strike),
                                 ext::make_shared<EuropeanExercise>(maturityDate));

            option.setPricingEngine(ext::make_shared<FdSabrVanillaEngine>(
                f0, alpha, beta, nu, rho, rTS, 25, 100, 50));

            const Volatility fdmVol =
                option.impliedVolatility(option.NPV(), bsProcess);

            const Real hagenVol = sabrVolatility(
                strike, f0, maturityTime, alpha, beta, nu, rho);

            const Real diff = std::fabs(fdmVol - hagenVol);

            if (std::fabs(fdmVol-hagenVol) > tol) {
                BOOST_ERROR(
                    "large difference between Hagen formula and FDM"
                    << "\n    strike          : " << strike
                    << "\n    option type     : "
                    << ((optionType == Option::Call) ? "Call" : "Put")
                    << "\n    Hagen vol       : " << hagenVol
                    << "\n    pde vol         : " << fdmVol
                    << "\n    vol difference  : " << diff
                    << "\n    tolerance       : " << tol);
            }
        }
    }
}

namespace {
    /*
     * Example and reference values are taken from
     * B. Chen, C.W. Oosterlee, H. Weide,
     * Efficient unbiased simulation scheme for the SABR stochastic volatility model.
     * https://http://ta.twi.tudelft.nl/mf/users/oosterle/oosterlee/SABRMC.pdf
     */

    class OsterleeReferenceResults {
      public:
        explicit OsterleeReferenceResults(Size i) : i_(i) { }

        Real operator()(Real t) const {
            Size i;
            if (close_enough(t, 1/16.))
                i = 0;
            else if (close_enough(t, 1/32.))
                i = 1;
            else
                QL_FAIL("unmatched reference result lookup");

            return data_[i_][i];
        }

      private:
        const Size i_;
        static Real data_[9][3];
    };

    Real OsterleeReferenceResults::data_[9][3] = {
        { 0.0610, 0.0604 }, { 0.0468, 0.0463 }, { 0.0347, 0.0343 },
        { 0.0632, 0.0625 }, { 0.0512, 0.0506 }, { 0.0406, 0.0400 },
        { 0.0635, 0.0630 }, { 0.0523, 0.0520 }, { 0.0422, 0.0421 }
    };
}

void FdSabrTest::testOosterleeTestCaseIV() {
    BOOST_TEST_MESSAGE("Testing Chen, Oosterlee and Weide test case IV...");

    SavedSettings backup;

    const Date today = Date(8, January, 2019);
    const DayCounter dc = Actual365Fixed();
    Settings::instance().evaluationDate() = today;

    const Handle<YieldTermStructure> rTS =
        Handle<YieldTermStructure>(flatRate(today, 0.0, dc));

    const Real f0    =  0.07;
    const Real alpha =  0.4;
    const Real nu    =  0.8;
    const Real beta  =  0.4;
    const Real rho   = -0.6;

    const Period maturities[] = {
        Period(2, Years), Period(5, Years), Period(10, Years)
    };

    const Real strikes[] = { 0.4*f0, f0, 1.6*f0 };

    const Real tol = 0.00035;
    for (Size i=0; i < LENGTH(maturities); ++i) {
        const Date maturityDate = today + maturities[i];
        const Time maturityTime = dc.yearFraction(today, maturityDate);

        const Size timeSteps = Size(5*maturityTime);

        const ext::shared_ptr<PricingEngine> engine =
            ext::make_shared<FdSabrVanillaEngine>(
                f0, alpha, beta, nu, rho, rTS, timeSteps, 200, 21);

        const ext::shared_ptr<Exercise> exercise =
            ext::make_shared<EuropeanExercise>(maturityDate);

        for (Size j=0; j < LENGTH(strikes); ++j) {
            const ext::shared_ptr<StrikedTypePayoff> payoff =
                ext::make_shared<PlainVanillaPayoff>(Option::Call, strikes[j]);

            VanillaOption option(payoff, exercise);
            option.setPricingEngine(engine);

            const Real calculated = option.NPV();

            const OsterleeReferenceResults referenceResuts(i*3+j);

            const Real expected = RichardsonExtrapolation(
                ext::function<Real(Real)>(referenceResuts), 1/16., 1)(2.);

            const Real diff = std::fabs(calculated - expected);
            if (diff > tol) {
                BOOST_ERROR(
                    "can not reproduce reference values from Monte-Carlo"
                    << "\n    strike     : " << payoff->strike()
                    << "\n    maturity   : " << maturityDate
                    << "\n    reference  : " << expected
                    << "\n    calculated : " << calculated
                    << "\n    difference : " << diff
                    << "\n    tolerance  : " << tol);
            }
        }
    }
}

void FdSabrTest::testBenchOpSabrCase() {
    BOOST_TEST_MESSAGE("Testing SABR BenchOp problem...");

    /*
     * von Sydow, L, Milovanović, S, Larsson, E, In't Hout, K,
     * Wiktorsson, M, Oosterlee, C.W, Shcherbakov, V, Wyns, M,
     * Leitao Rodriguez, A, Jain, S, et al. (2018)
     * BENCHOP–SLV: the BENCHmarking project in Option
     * Pricing–Stochastic and Local Volatility problems
     * https://ir.cwi.nl/pub/28249
     */

    SavedSettings backup;

    const Date today = Date(8, January, 2019);
    const DayCounter dc = Actual365Fixed();
    Settings::instance().evaluationDate() = today;

    const Handle<YieldTermStructure> rTS =
        Handle<YieldTermStructure>(flatRate(today, 0.0, dc));

    const Size maturityInYears[] = { 2, 10 };

    const Real f0s[]    = { 0.5, 0.07 };
    const Real alphas[] = { 0.5, 0.4 };
    const Real nus[]    = { 0.4, 0.8 };
    const Real betas[]  = { 0.5, 0.5 };
    const Real rhos[]   = { 0.0, -0.6 };

    const Real expected[2][3] = {
        { 0.221383196830866, 0.193836689413803, 0.166240814653231 },
        { 0.052450313614407, 0.046585753491306, 0.039291470612989 }
    };

    const Size gridX = 400;
    const Size gridY = 25;
    const Size gridT = 10;

    const Real factor = 2;

    const Real tol = 2e-4;

    for (Size i=0; i < LENGTH(f0s); ++i) {

        const Date maturity = today + Period(maturityInYears[i]*365, Days);
        const Time T = dc.yearFraction(today, maturity);

        const Real f0    = f0s[i];
        const Real alpha = alphas[i];
        const Real nu    = nus[i];
        const Real beta  = betas[i];
        const Real rho   = rhos[i];

        const Real strikes[] = {
            f0*std::exp(-0.1*std::sqrt(T)), f0, f0*std::exp(0.1*std::sqrt(T))
        };

        for (Size j=0; j < LENGTH(strikes); ++j) {
            const Real strike = strikes[j];

            VanillaOption option(
                ext::make_shared<PlainVanillaPayoff>(Option::Call, strike),
                ext::make_shared<EuropeanExercise>(maturity));

            option.setPricingEngine(ext::make_shared<FdSabrVanillaEngine>(
                    f0, alpha, beta, nu, rho, rTS,
                    Size(gridT*factor),
                    Size(gridX*factor),
                    Size(gridY*std::sqrt(factor))));

            const Real calculated = option.NPV();
            const Real diff = std::fabs(calculated - expected[i][j]);

            if (diff > tol) {
                BOOST_ERROR(
                    "failed to reproduce reference values"
                    << "\n    strike     : " << strike
                    << "\n    maturity   : " << maturity
                    << "\n    reference  : " << expected[i][j]
                    << "\n    calculated : " << calculated
                    << "\n    difference : " << diff
                    << "\n    tolerance  : " << tol);
            }
        }
    }
}

test_suite* FdSabrTest::suite(SpeedLevel speed) {
    auto* suite = BOOST_TEST_SUITE("Finite Difference SABR tests");

    suite->add(QUANTLIB_TEST_CASE(&FdSabrTest::testFdmSabrCevPricing));
    suite->add(QUANTLIB_TEST_CASE(&FdSabrTest::testFdmSabrVsVolApproximation));
    suite->add(QUANTLIB_TEST_CASE(&FdSabrTest::testOosterleeTestCaseIV));
    suite->add(QUANTLIB_TEST_CASE(&FdSabrTest::testBenchOpSabrCase));

    if (speed <= Fast) {
        suite->add(QUANTLIB_TEST_CASE(&FdSabrTest::testFdmSabrOp));
    }

    return suite;
}


/*
 correlation.cxx

 Copyright (c) 2018 Guy Skinner

 This file is distributed under the terms of the MIT license.
 Please see the file 'LICENCE.txt' in the root directory
 or http://opensource.org/licenses/mit-license.php for information.
*/

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

#include "correlation.hxx"
#include "neighbour.hxx"
#include "supercell.hxx"
#include "utils.hxx"

Correlation::Correlation() {
}

void Correlation::Calculate(const Neighbour& neighbour_table,
                            Supercell& supercell) {

  ublas::matrix<double> table = neighbour_table.Sites;

  std::vector<double> pairs = {};
  std::vector<long> count = {};
  std::vector<double> corrs = {};

  auto nsum = 0;

  for (auto i = 0; i < supercell.number_of_atoms; i++) {
    ublas::matrix_row<ublas::matrix<double>> ib(supercell.basis_vectors,i);
    auto ip = supercell.pointers(i);
    for (auto j = 0; j < neighbour_table.Total(i); j++) {
      auto ptr = neighbour_table.Pointers(nsum+j);
      auto jbsp = neighbour_table.BasisPointers(ptr);
      auto jp = supercell.pointers(jbsp);

      ublas::matrix_row<ublas::matrix<double>> jb(table,ptr);

      double r = ublas::norm_2(jb-ib);
      double corr = static_cast<double>(ip*jp);
      long index = linear_search(pairs,r,1e-6);

      if (index == pairs.size()) {
        pairs.insert(pairs.begin(),r);
        count.insert(count.begin(),1);
        corrs.insert(corrs.begin(),corr);
      } else {
        count[index]++;
        corrs[index] += corr;
      }
    }
    nsum += neighbour_table.Total(i);
  }

  /* Normalize */
  for (auto i = 0; i < pairs.size(); i++) {
    corrs[i] /= count[i];
  }

  /* Sorting */
  std::vector<std::size_t> indices(pairs.size());
  std::iota(indices.begin(),indices.end(),0);
  std::sort(indices.begin(), indices.end(),
            [&pairs](std::size_t left, std::size_t right) {
              return pairs[left] < pairs[right];
            });

  std::vector<double> sort_pairs = pairs;
  std::vector<long> sort_count = count;
  std::vector<double> sort_corrs = corrs;

  number = pairs.size();
  for (auto i = 0; i < number; i++) {
    sort_pairs[i] = pairs[indices[i]];
    sort_count[i] = count[indices[i]];
    sort_corrs[i] = corrs[indices[i]];
  }

  pair_clusters = sort_pairs;
  pair_count = sort_count;
  pair_correlations = sort_corrs;

}

double Correlation::ErrorFunction(double x) {

  double error = (2*x-1)*(2*x-1);
  ublas::vector<double> efs(number);

  for (auto i = 0; i < number; i++) {
    efs(i) = std::fabs(error-pair_correlations[i]);
  }

  errors = efs;
  return ublas::sum(efs)/number;

}


//////////////////////////////////////////////////////////////////////////////
// random::students_t_distribution.hpp                                      //
//                                                                          //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_RANDOM_STUDENTS_T_HPP_ER_2009
#define BOOST_RANDOM_STUDENTS_T_HPP_ER_2009
#include <vector>
#include <algorithm>
#include <iterator>
#include <boost/range.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/chi_squared.hpp>

namespace boost{
namespace random{

    // Samples from a students_t distribution
    template<typename T>
    class students_t_distribution{
            typedef boost::normal_distribution<T>           nd_;
            typedef random::chi_squared_distribution<T>     cs_;
        public:
            typedef typename nd_::input_type input_type;
            typedef typename nd_::result_type result_type;

        students_t_distribution():df_(2){}
        students_t_distribution(unsigned df):df_(df){}

        template<typename U>
        result_type
        operator()(U& urng){
            static nd_ nd(0,1);
            static cs_ cs(df_);
            result_type z = nd(urng);
            result_type d = cs(urng);
            d /= static_cast<result_type>(df_);
            d = sqrt(d);
            return z / d;
        }

        unsigned df()const{ return df_; }

        private:
            unsigned df_;
    };

}// random
}// boost

#endif


/*
 * Copyright (c) 2016-2018 Nicholas Corgan (n.corgan@gmail.com)
 *
 * Distributed under the MIT License (MIT) (See accompanying file LICENSE.txt
 * or copy at http://opensource.org/licenses/MIT)
 */

#include "../exception_internal.hpp"
#include "../utils/misc.hpp"
#include "../database/database_common.hpp"

#include <pkmn/config.hpp>
#include <pkmn/exception.hpp>
#include <pkmn/calculations/moves/hidden_power.hpp>

#include <boost/config.hpp>

#include <cmath>

namespace pkmn { namespace calculations {

    // Most significant bit
    #define MSB(var) (((var) >> 3) & 1)

    /*
     * There is no Normal-type Hidden Power, so all type indices
     * are offset from normal.
     */
    inline pkmn::e_type gen2_hidden_power_type(
        int IV_attack, int IV_defense
    )
    {
        return static_cast<pkmn::e_type>(
                   (4 * (IV_attack % 4) + (IV_defense % 4)) + 2
               );
    }

    inline int gen2_hidden_power_base_power(
        uint8_t v, uint8_t w, uint8_t x,
        uint8_t y, uint8_t Z
    )
    {
        return int(std::floor<int>(((5 * (v + (w<<1) + (x<<2) + (y<<3)) + Z) / 2) + 31));
    }

    hidden_power gen2_hidden_power(
        int IV_attack,
        int IV_defense,
        int IV_speed,
        int IV_special
    ) {
        // Input validation
        pkmn::enforce_IV_bounds("Attack",  IV_attack,  false);
        pkmn::enforce_IV_bounds("Defense", IV_defense, false);
        pkmn::enforce_IV_bounds("Speed",   IV_speed,   false);
        pkmn::enforce_IV_bounds("Special", IV_special, false);

        uint8_t v = MSB(IV_special);
        uint8_t w = MSB(IV_speed);
        uint8_t x = MSB(IV_defense);
        uint8_t y = MSB(IV_attack);
        uint8_t Z = (IV_special % 4);

        return hidden_power(
                   gen2_hidden_power_type(IV_attack, IV_defense),
                   gen2_hidden_power_base_power(v, w, x, y, Z)
               );
    }

    // Least significant bit
    #define LSB(var)  ((var) & 1)
    // Second-least significant bit
    #define LSB2(var) (((var) & 2) >> 1)

    inline pkmn::e_type modern_hidden_power_type(
        uint8_t a, uint8_t b, uint8_t c,
        uint8_t d, uint8_t e, uint8_t f
    )
    {
         return static_cast<pkmn::e_type>(static_cast<int>(
                    (std::floor<int>(((a + (b<<1) + (c<<2) + (d<<3) + (e<<4) + (f<<5)) * 15) / 63)) + 2
                ));
    }

    inline int modern_hidden_power_base_power(
        uint8_t u, uint8_t v, uint8_t w,
        uint8_t x, uint8_t y, uint8_t z
    )
    {
        return int(std::floor<int>((((u + (v<<1) + (w<<2) + (x<<3) + (y<<4) + (z<<5)) * 40) / 63) + 30));
    }

    hidden_power modern_hidden_power(
        int IV_HP,
        int IV_attack,
        int IV_defense,
        int IV_speed,
        int IV_spatk,
        int IV_spdef
    )
    {
        // Input validation
        pkmn::enforce_IV_bounds("HP",              IV_HP,      true);
        pkmn::enforce_IV_bounds("Attack",          IV_attack,  true);
        pkmn::enforce_IV_bounds("Defense",         IV_defense, true);
        pkmn::enforce_IV_bounds("Speed",           IV_speed,   true);
        pkmn::enforce_IV_bounds("Special Attack",  IV_spatk,   true);
        pkmn::enforce_IV_bounds("Special Defense", IV_spdef,   true);

        uint8_t a = LSB(IV_HP);
        uint8_t b = LSB(IV_attack);
        uint8_t c = LSB(IV_defense);
        uint8_t d = LSB(IV_speed);
        uint8_t e = LSB(IV_spatk);
        uint8_t f = LSB(IV_spdef);

        uint8_t u = LSB2(IV_HP);
        uint8_t v = LSB2(IV_attack);
        uint8_t w = LSB2(IV_defense);
        uint8_t x = LSB2(IV_speed);
        uint8_t y = LSB2(IV_spatk);
        uint8_t z = LSB2(IV_spdef);

        return hidden_power(
                   modern_hidden_power_type(a, b, c, d, e, f),
                   modern_hidden_power_base_power(u, v, w, x, y, z)
               );
    }

}}


// Component
#include "ForwardSimHelper.hpp"

// Ros
#include <tf/tf.h>
#include <tf/transform_datatypes.h>

// Libraries
#include <boost/cstdfloat.hpp>
#include <boost/shared_ptr.hpp>

// Standard
#include <utility>

namespace cm
{

using float64_t = boost::float64_t; ///< Alias for 64 bit float

nav_msgs::Odometry::ConstPtr ForwardSimHelper::forwardSimPose(const nav_msgs::Odometry::ConstPtr& pose, const ros::Time& now_s)
{
    const ros::Duration dt_s = pose->header.stamp - now_s;

    nav_msgs::Odometry new_pose = *pose;

    const float64_t x_mps = pose->twist.twist.linear.x;
    const float64_t y_mps = pose->twist.twist.linear.y;
    const float64_t z_mps = pose->twist.twist.linear.z;

    new_pose.pose.pose.position.x = pose->pose.pose.position.x + x_mps*dt_s.toSec();
    new_pose.pose.pose.position.y = pose->pose.pose.position.y + y_mps*dt_s.toSec();
    new_pose.pose.pose.position.z = pose->pose.pose.position.z + z_mps*dt_s.toSec();

    tf::Quaternion q;
    tf::quaternionMsgToTF(pose->pose.pose.orientation, q);
    float64_t roll_r, pitch_r, yaw_r;
    tf::Matrix3x3(std::move(q)).getRPY(roll_r, pitch_r, yaw_r);

    roll_r  += pose->twist.twist.angular.x*dt_s.toSec();
    pitch_r += pose->twist.twist.angular.y*dt_s.toSec();
    yaw_r   += pose->twist.twist.angular.z*dt_s.toSec();

    new_pose.twist.twist.linear.x =  x_mps*std::cos(yaw_r) + y_mps*std::sin(yaw_r);
    new_pose.twist.twist.linear.y = -x_mps*std::sin(yaw_r) + y_mps*std::cos(yaw_r);

    geometry_msgs::Quaternion new_q;
    tf::quaternionTFToMsg(tf::createQuaternionFromRPY(roll_r, pitch_r, yaw_r), new_q);
    new_pose.pose.pose.orientation = new_q;

    return boost::make_shared<nav_msgs::Odometry>(new_pose);
}

} // namespace cm


/*****************************************************************************
*
* Copyright (C) 2015-2020 by Synge Todo <wistaria@phy.s.u-tokyo.ac.jp>
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

// Calculating free energy, energy, and specific heat of square lattice Ising model

#include <iomanip>
#include <iostream>
#include <boost/lexical_cast.hpp>
#include "square/infinite.hpp"

int main(int argc, char **argv) {
  typedef double real_t;
  real_t Jx, Jy, t_min, t_max, t_step;
  if (argc == 6) {
    Jx = boost::lexical_cast<real_t>(argv[1]);
    Jy = boost::lexical_cast<real_t>(argv[2]);
    t_min = boost::lexical_cast<real_t>(argv[3]);
    t_max = boost::lexical_cast<real_t>(argv[4]);
    t_step = boost::lexical_cast<real_t>(argv[5]);
  } else if (argc == 1) {
    std::cin >> Jx >> Jy >> t_min >> t_max >> t_step;
  } else {
    std::cerr << "Usage: " << argv[0] << " [Jx Jy t_min t_max t_step]\n";
    return 127;
  }
  std::cout << std::scientific << std::setprecision(std::numeric_limits<real_t>::digits10);
  std::cout << "# square lattice Ising model\n";
  std::cout << "# Jx, Jy, T, free energy density, energy density, specific heat\n";
  for (real_t t = t_min; t <= t_max; t += t_step) {
    real_t beta = 1 / t;
    auto result = ising::square::infinite(beta, Jx, Jy);
    std::cout << Jx << ' ' << Jy << ' ' << t << ' ' << std::get<0>(result) << ' '
              << std::get<1>(result) << ' ' << std::get<2>(result) << std::endl;
  }
}


// gray_code_iterator.hpp
//
// Produces all n-tuples of {0,1} in a minimal change ordering
// (i.e. Gray code). In particular, two consecutive elements
// have Hamming distance equal to 1. The algorithm is a loopless
// generation method described in:
//
//   Bitner, James R., Gideon Ehrlich, and Edward M. Reingold.
//   "Efficient generation of the binary reflected Gray code and its applications."
//   Communications of the ACM 19.9 (1976): 517-521.

#ifndef GRAY_CODE_ITERATOR_HPP
#define GRAY_CODE_ITERATOR_HPP

#include <cstdint>
#include <numeric>
#include <type_traits>

#include <boost/iterator/iterator_facade.hpp>

template <typename T>
class gray_code_iterator
	: public boost::iterator_facade <
	gray_code_iterator<T>,
	const T&,
	boost::forward_traversal_tag
	>
{
private:
	typedef std::uint_fast8_t focus_ptr_t;
	static_assert(std::is_integral<T>::value, "T must be integral");

public:
	gray_code_iterator() : end_(true), n_(0), a_(0), f_(0) { }

	explicit gray_code_iterator(int n) : end_(false), n_(n), a_(0), f_(new focus_ptr_t[n + 1])
	{
		assert(n <= sizeof(T) * 8 && "T not large enough to hold n tuples");

		std::iota(f_, f_ + n + 1, 0);

		assert(a_ == 0);
	}

	~gray_code_iterator()
	{
		if (f_)
		{
			delete[] f_;
		}
	}

private:
	friend class boost::iterator_core_access;

	void increment()
	{
		const focus_ptr_t j = f_[0];

		if (j == n_)
		{
			end_ = true;
			return;
		}

		f_[0] = 0;
		f_[j] = f_[j + 1];
		f_[j + 1] = j + 1;

		a_ ^= (1 << j);
	}

	bool equal(const gray_code_iterator& other) const
	{
		return end_ == other.end_;
	}

	const T& dereference() const
	{
		return a_;
	}

	bool end_;
	const int n_;
	T a_;
	focus_ptr_t* f_;
};

#endif


//! [reduc]
#include <boost/simd/constant/zero.hpp>
#include <iostream>
#include <numeric>
#include <vector>

#include <boost/simd/meta/cardinal_of.hpp>
#include <boost/simd/pack.hpp>
//! [reduc-inc]
#include <boost/simd/function/sum.hpp>
//! [reduc-inc]

int main()
{
  //! [reduc-simd-types]
  namespace bs = boost::simd;
  using pack_t = bs::pack<int32_t>;

  constexpr std::size_t size = 64;
  std::int32_t card_int      = bs::cardinal_of<pack_t>();

  std::vector<int32_t> array(size);
  std::iota(array.begin(), array.end(), 0);
  //! [reduc-simd-types]

  //! [reduc-scalar]
  // Scalar version
  int32_t sum = 0;
  for (size_t i = 0; i < size; ++i) {
    sum += array[i];
  }
  //! [reduc-scalar]
  std::cout << "Scalar sum for size " << size << " is " << sum << std::endl;

  //! [reduc-simd-l]
  sum = 0;
  bs::pack<int32_t, size> array_pack(array.data());
  sum = bs::sum(array_pack);
  //! [reduc-simd-l]
  std::cout << "SIMD sum 1 for size " << size << " is " << sum << std::endl;

  //! [reduc-simd-o]
  sum = 0;
  pack_t sum_p{0};
  for (size_t i = 0; i < size; i += card_int) {
    sum_p += pack_t(array.data() + i);
  }
  sum = bs::sum(sum_p);
  //! [reduc-simd-o]
  std::cout << "SIMD sum 2 for size " << size << " is " << sum << std::endl;

  //! [reduc-simd-r]
  // The input data is an arbitrary size
  size_t newsize = size + 13;
  array.resize(newsize);
  std::iota(array.begin(), array.end(), 0);

  sum_p    = bs::Zero<pack_t>();
  size_t i = 0;
  for (; i + card_int <= newsize; i += card_int) {
    sum_p += pack_t(array.data() + i);
  }
  sum = bs::sum(sum_p);
  for (; i < newsize; ++i) {
    sum += array[i];
  }
  //! [reduc-simd-r]

  std::cout << "SIMD sum 3 for size " << newsize << " is " << sum << std::endl;

  return 0;
}
//! [reduc-compile]
// This code can be compiled using (for instance for gcc)
// g++ reduction.cpp -msse4.2 -std=c++11 -O3 -DNDEBUG -o reduction
// -I/path_to/boost_simd/ -I/path_to/boost/
//! [reduc-compile]
//! [reduc]


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_

#include <boost/multiprecision/cpp_int.hpp>

using namespace boost::multiprecision;

number<cpp_int_backend<> > a;
number<cpp_int_backend<>, et_off> b;
number<cpp_int_backend<64, 64, signed_magnitude, checked, void>, et_off> c;
number<cpp_int_backend<128, 128, signed_magnitude, checked, void>, et_off> d;
number<cpp_int_backend<500, 500, signed_magnitude, checked, void>, et_off> e;


#include "spdlog/spdlog.h"
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <limits>
#include <mimkl/data_structures.hpp>
#include <mimkl/definitions.hpp>
#include <mimkl/io.hpp>
#include <mimkl/kernels.hpp>
#include <mimkl/linear_algebra.hpp>
#include <mimkl/models/easy_mkl.hpp>
#include <mimkl/solvers/komd.hpp>
#include <mimkl/utilities.hpp>
#include <numeric>
#include <spdlog/fmt/ostr.h>
#include <utility>

using dlib::mat;
using mimkl::data_structures::DataFrame;
using mimkl::data_structures::indexing_from_vector;
using mimkl::data_structures::range;
using mimkl::definitions::Indexing;
using mimkl::utilities::check_invocable;
using mimkl::utilities::print_type;

//#define SPDLOG_DEBUG_ON
//#define SPDLOG_TRACE_ON

// Compile time log levels
// define SPDLOG_DEBUG_ON or SPDLOG_TRACE_ON
// SPDLOG_TRACE(console, "Enabled only #ifdef SPDLOG_TRACE_ON..{} ,{}",
// 1, 3.23);  SPDLOG_DEBUG(console, "Enabled only #ifdef SPDLOG_DEBUG_ON.. {}
// ,{}", 1, 3.23);

int main(int argc, char **argv)
{

    // Runtime log levels
    spdlog::set_level(spdlog::level::trace); // Set global log level to info
    auto console = spdlog::stdout_color_mt("console");

    const Index rows = 4; // 3 to reproduce single member in class  error
    const Index dims = 2;

    MATRIX(double) X(rows, 2);
    //	X << 1., 1., 3., 1., 1., 2.;
    X << 1., 1., 3., 1., 1., 4., 3., 2.;
    console->info("X\n{}", X);

    std::vector<std::string> labels;
    labels.reserve(rows);
    labels.push_back("a");
    labels.push_back("b");
    labels.push_back("a");
    labels.push_back("b");

    Eigen::SparseMatrix<double> L(dims, dims);
    mimkl::linear_algebra::fill_sparse_diagonal(L, 1.0);

    Eigen::SparseMatrix<double> L1(dims, dims);
    typedef Eigen::Triplet<double> TripletDouble; // (row,col,coef)
    std::vector<TripletDouble> triplet_list;
    triplet_list.reserve(4);
    triplet_list.push_back(TripletDouble(0, 1, 1.));
    //  triplet_list.push_back(TripletDouble(1, 2, 1.));
    triplet_list.push_back(TripletDouble(1, 0, 1.));
    //  triplet_list.push_back(TripletDouble(2, 1, 1.));
    L1.setFromTriplets(triplet_list.begin(), triplet_list.end());

    MATRIX(double)
    K_ref0 = mimkl::induction::induce_linear_kernel<MATRIX(double)>(X, X, L);
    console->info("a kernel:\n{}", K_ref0);
    //
    MATRIX(double)
    K_ref1 =
    mimkl::induction::induce_polynomial_kernel<MATRIX(double)>(X, X, L1, 1, 0);
    console->info("another kernel:\n{}", K_ref1);

    std::vector<Eigen::SparseMatrix<double>> inducer_vec;
    inducer_vec.reserve(2);
    inducer_vec.push_back(L);
    inducer_vec.push_back(L1);

    typedef std::function<MATRIX(double)(const MATRIX(double) &, const MATRIX(double) &,
                                         const Eigen::SparseMatrix<double>)>
    InducerFunction;
    typedef std::function<MATRIX(double)(const MATRIX(double) &,
                                         const MATRIX(double) &)>
    InducedFunction;

    const double degree = 1.;
    const double offset = 0.;

    InducerFunction k_poly =
    [degree, offset](const MATRIX(double) & lhs, const MATRIX(double) & rhs,
                     const Eigen::SparseMatrix<double> inducer) {
        return mimkl::induction::induce_polynomial_kernel<MATRIX(double)>(
        lhs, rhs, inducer, degree, offset);
    };

    MATRIX(double) K(rows, rows);
    console->info("\e[1;32mtype: \e[0m \n{}",
                  print_type<decltype(k_poly(X, X, L))>());
    console->info("decltype argument evaluated? (no):\n{}", K);
    console->info("\e[1;32mtype: \e[0m \n{}", print_type(k_poly));
    console->info("check_invocable? {}", check_invocable(k_poly));

    std::vector<InducedFunction> function_vec =
    mimkl::induction::inducer_combination(k_poly, inducer_vec);

    MATRIX(double) K2 = function_vec[0](X, X);
    console->info("a kernel from function:\n{}", K2);

    MATRIX(double) K3 = function_vec[1](X, X);
    console->info("another kernel from function:\n{}", K3);

    assert(((K_ref0 - K2).norm() == 0) && "Identity Inducer");
    assert(((K_ref1 - K3).norm() == 0) && "non-identity Inducer");

    console->info("\e[1;35m not callable"); //-----------
    console->info("int is not invocable! {}",
                  mimkl::utilities::is_invocable<int>::value);

    console->info("\e[1;35m function pointer"); //-----------
    // lambda to function pointer only without any captures
    void (*foo)(int) = [](int a) {};
    console->info("\e[1;32mtype: \e[0m \n{}", print_type(foo));
    console->info("function pointer check_invocable?: {}", check_invocable(foo));
    console->info("function pointer is_function_t?: {}",
                  mimkl::utilities::is_function_t<decltype(foo)>::value);

    std::function<void(int)> bar = foo;
    console->info("\e[1;32mtype: \e[0m \n{}", print_type(bar));
    console->info("function pointer cast to std::function invocable?: {}",
                  check_invocable(bar));

    console->info("\e[1;35m lambdas"); //-----------
    auto lam = []() {};
    console->info("\e[1;32mtype: \e[0m \n{}", print_type(lam));
    console->info("is lambda invocable? {}",
                  mimkl::utilities::is_invocable<decltype(lam)>::value);
    console->info("\e[1;32mtype: \e[0m \n{}", print_type([]() {}));
    console->info("check_invocable? {}", check_invocable([]() {}));

    console->info("\e[1;35m function object"); //-----------
    struct FunctionObject
    {
        int operator()(int a) { return a; };
    };
    FunctionObject fo;
    console->info("\e[1;32mtype: \e[0m \n{}", print_type(fo));
    console->info("is invocable? {}",
                  mimkl::utilities::is_invocable<FunctionObject>::value);
    console->info("check invocable {}", check_invocable(fo));

    console->info("\e[1;35m std::function<>"); //-----------
    //  console->info("\e[1;32mtype: \e[0m \n{}",
    //  print_type<mimkl::induction::induce_polynomial_kernel>());
    console->info("\e[1;32mtype: \e[0m \n{}",
                  print_type<std::function<void(int)>>());
    console->info("is invocable? {}",
                  mimkl::utilities::is_invocable<std::function<void(int)>>::value);

    console->info("\e[1;35m function_vec element"); //-----------
    console->info("\e[1;32mtype: \e[0m \n{}", print_type(function_vec[0]));
    console->info("element check_invocable {}", check_invocable(function_vec[0]));
    console->info("element is invocable? {}",
                  mimkl::utilities::is_invocable<decltype(function_vec[0])>::value);
    console->info("element is_function? {}",
                  std::is_function<decltype(function_vec[0])>::value);
    console->info("is_function_t ? {}",
                  mimkl::utilities::is_function_t<decltype(function_vec[0])>::value);
    //  console->info("is invocable?
    //  {}",mimkl::utilities::is_invocable<decltype(mimkl::induction::inducer_combination)>::value);

    console->info("\e[1;35m function_vec::value_type"); //-----------
    console->info("\e[1;32mtype: \e[0m \n{}", print_type(function_vec));
    console->info("\e[1;32mtype: \e[0m \n{}",
                  print_type<decltype(function_vec)::value_type>());
    console->info("is vector type std::function? {}",
                  std::is_function<decltype(function_vec)::value_type>::value);
    console->info(
    "is vector type invocable? {}",
    mimkl::utilities::is_invocable<decltype(function_vec)::value_type>::value);

    console->info("\e[1;35m Eigen::Matrix"); //-----------
    console->info("\e[1;32mtype: \e[0m \n{}", print_type(K2));
    console->info("\e[1;32mtype: \e[0m \n{}", print_type<MATRIX(double)>());
    console->info("is std::function? {}", std::is_function<decltype(K2)>::value);
    console->info("is invocable? {}",
                  mimkl::utilities::is_invocable<decltype(K2)>::value);
    console->info("check_invocable {}", check_invocable(K2));

    console->info(
    "\e[1;35m checkout move semantics of Eigen"); //-----------checkout move
    // semantics
    std::function<MATRIX(double)()> spaghetti_monster = [console]() {
        MATRIX(double) B = MATRIX(double)::Constant(3, 3, 42);
        auto B_ptr = (void *)B.data();
        console->info("pointer to B {}", B_ptr);
        return B;
    };
    std::function<MATRIX(double)()> illuminati = [console]() {
        return MATRIX(double)::Constant(3, 3, 1) +
               MATRIX(double)::Constant(3, 3, 22);
    };

    MATRIX(double) A = 10 * MATRIX(double)::Random(3, 3);
    console->info("A {}", A);
    console->info("type of A.data():  {}", print_type(A.data()));
    console->info("type of (void *)A.data():  {}", print_type((void *)A.data()));
    auto A_ptr = (void *)A.data();
    console->info("pointer to A {}", A_ptr);

    A = spaghetti_monster();
    console->info("nooo, the spaghetti_monster!");
    auto C_ptr = (void *)A.data();
    console->info("pointer to A, is B? {}", C_ptr);

    double *A1_ptr = A.data();
    console->info("pointer to A (same, eh) {}", (void *)A1_ptr);
    MATRIX(double) D;
    D = std::move(A);
    double *D_ptr = D.data();
    console->info("pointer to D, is moved A? {}", (void *)D_ptr);
    console->info("D \n{}", D);

    D = illuminati();
    console->info("nooo, the illuminati!");
    double *E_ptr = D.data();
    console->info("pointer to D, is prolly pointer to temporary of evalueted "
                  "expression?{}",
                  (void *)E_ptr);
    console->info("E \n{}", D);
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2019 - 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.
 *
 * ---------------------------------------------------------------------
 * This tutorial program was contributed by Martin Kronbichler
 */

// @sect3{Include files}

// 本教程的包含文件与  step-6  中的基本相同。重要的是，我们将使用的TransfiniteInterpolationManifold类是由`deal.II/grid/manifold_lib.h`提供。

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

#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/vector.h>

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

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

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>

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

#include <fstream>

// 唯一的新include文件是MappingQCache类的文件。

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

namespace Step65
{
  using namespace dealii;
// @sect3{Analytical solution and coefficient}

// 在这个教程程序中，我们要解决泊松方程，其系数沿半径为0.5的球体跳跃，并使用一个恒定的右手边值 $f(\mathbf{x}) = -3$  。（这个设置与 step-5 和 step-6 相似，但系数和右手边的具体数值不同）。由于系数的跳跃，分析解必须有一个结点，即系数从一个值切换到另一个值。为了保持简单，我们选择了一个在所有分量中都是二次的分析解，即在半径为0.5的球中为 $u(x,y,z) = x^2 + y^2 + z^2$ ，在域的外部为 $u(x,y,z) = 0.1(x^2 + y^2 + z^2) + 0.25-0.025$ 。这个分析解在内球的系数为0.5，外球的系数为5的情况下与右手边兼容。它也是沿着半径为0.5的圆连续的。

  template <int dim>
  class ExactSolution : public Function<dim>
  {
  public:
    virtual double value(const Point<dim> &p,
                         const unsigned int /*component*/ = 0) const override
    {
      if (p.norm_square() < 0.25)
        return p.norm_square();
      else
        return 0.1 * p.norm_square() + (0.25 - 0.025);
    }

    virtual Tensor<1, dim>
    gradient(const Point<dim> &p,
             const unsigned int /*component*/ = 0) const override
    {
      if (p.norm_square() < 0.25)
        return 2. * p;
      else
        return 0.2 * p;
    }
  };

  template <int dim>
  double coefficient(const Point<dim> &p)
  {
    if (p.norm_square() < 0.25)
      return 0.5;
    else
      return 5.0;
  }

//  @sect3{The PoissonProblem class}

// 泊松问题的实现与我们在  step-5  教程中使用的非常相似。两个主要的区别是，我们向程序中的各个步骤传递了一个映射对象，以便在两种映射表示法之间进行切换，正如介绍中所解释的那样，还有一个`计时器'对象（TimerOutput类型），将用于测量各种情况下的运行时间。(映射对象的概念在 step-10 和 step-11 中首次提出，如果你想查一下这些类的用途的话)。

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

  private:
    void create_grid();
    void setup_system(const Mapping<dim> &mapping);
    void assemble_system(const Mapping<dim> &mapping);
    void solve();
    void postprocess(const Mapping<dim> &mapping);

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

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

    TimerOutput timer;
  };

// 在构造函数中，我们设置了定时器对象来记录墙的时间，但在正常执行过程中是安静的。我们将在 `PoissonProblem::run()` 函数中查询它的计时细节。此外，我们为正在使用的有限元选择了一个相对较高的多项式三度。

  template <int dim>
  PoissonProblem<dim>::PoissonProblem()
    : fe(3)
    , dof_handler(triangulation)
    , timer(std::cout, TimerOutput::never, TimerOutput::wall_times)
  {}

//  @sect3{Grid creation and initialization of the manifolds}

// 接下来的函数介绍了TransfiniteInterpolationManifold的典型用法。第一步是创建所需的网格，这可以通过GridGenerator的两个网格的组合来完成。内球网格是很简单的。我们以原点为中心运行 GridGenerator::hyper_cube() ，半径为0.5（第三个函数参数）。第二个网格更有趣，构建方法如下。我们希望有一个在内部是球形的，但在外表面是平的网格。此外，内球的网格拓扑结构应该与外球的网格兼容，即它们的顶点重合，这样才能使两个网格合并起来。从 GridGenerator::hyper_shell 出来的网格满足了内侧的要求，如果它是用 $2d$ 的粗大单元创建的（在3D中我们将使用6个粗大单元）&ndash；这与球的边界面的单元数量相同。对于外表面，我们利用这样一个事实：没有流形附着的壳表面的6个面将退化为立方体的表面。我们仍然缺少的是外壳边界的半径。由于我们想要一个范围为 $[-1, 1]$ 的立方体，而6单元壳将其8个外顶点放在8条对角线上，我们必须将点 $(\pm 1, \pm 1, \pm 1)$ 转化为半径。显然，在 $d$ 维度上，半径必须是 $\sqrt{d}$ ，也就是说，对于我们要考虑的三维情况，半径是 $\sqrt{3}$ 。

// 这样，我们就有了一个计划。在创建了球的内部三角形和外壳的三角形之后，我们将这两个网格合并，但是将GridGenerator中的函数可能从产生的三角形中设置的所有流形移除，以确保我们对流形有充分的控制。特别是，我们希望在细化过程中在边界上添加的额外点能够遵循平坦的流形描述。为了开始添加更合适的流形ID的过程，我们给所有的网格实体（单元、面、线）分配流形ID 0，这些实体以后将与TransfiniteInterpolationManifold相关联。然后，我们必须识别沿着半径为0.5的球体的面和线，并给它们标记一个不同的流形ID，以便随后给这些面和线分配一个SphericalManifold。由于我们在调用 GridGenerator::hyper_ball(), 后丢弃了所有预先存在的流形，我们手动检查了网格的单元格和所有的面。如果四个顶点的半径都是0.5，我们就在球体上找到了一个面，或者像我们在程序中写的那样，有  $r^2-0.25 \approx 0$  。注意，我们调用`cell->face(f)->set_all_manifold_ids(1)`来设置面和周围线上的流形id。此外，我们希望通过一个材料ID来区分球内和球外的单元，以便于可视化，对应于介绍中的图片。

  template <int dim>
  void PoissonProblem<dim>::create_grid()
  {
    Triangulation<dim> tria_inner;
    GridGenerator::hyper_ball(tria_inner, Point<dim>(), 0.5);

    Triangulation<dim> tria_outer;
    GridGenerator::hyper_shell(
      tria_outer, Point<dim>(), 0.5, std::sqrt(dim), 2 * dim);

    GridGenerator::merge_triangulations(tria_inner, tria_outer, triangulation);

    triangulation.reset_all_manifolds();
    triangulation.set_all_manifold_ids(0);

    for (const auto &cell : triangulation.cell_iterators())
      {
        for (const auto &face : cell->face_iterators())
          {
            bool face_at_sphere_boundary = true;
            for (const auto v : face->vertex_indices())
              {
                if (std::abs(face->vertex(v).norm_square() - 0.25) > 1e-12)
                  face_at_sphere_boundary = false;
              }
            if (face_at_sphere_boundary)
              face->set_all_manifold_ids(1);
          }
        if (cell->center().norm_square() < 0.25)
          cell->set_material_id(1);
        else
          cell->set_material_id(0);
      }

// 有了所有单元格、面和线的适当标记，我们可以将流形对象附加到这些数字上。流形ID为1的实体将得到一个球形流形，而流形ID为0的其他实体将被分配到TransfiniteInterpolationManifold。正如介绍中提到的，我们必须通过调用 TransfiniteInterpolationManifold::initialize() 显式初始化当前网格的流形，以获取粗略的网格单元和连接到这些单元边界的流形。我们还注意到，我们在这个函数中本地创建的流形对象是允许超出范围的（就像它们在函数范围结束时那样），因为Triangulation对象在内部复制它们。

// 在连接了所有的流形之后，我们最后将去细化网格几次，以创建一个足够大的测试案例。

    triangulation.set_manifold(1, SphericalManifold<dim>());

    TransfiniteInterpolationManifold<dim> transfinite_manifold;
    transfinite_manifold.initialize(triangulation);
    triangulation.set_manifold(0, transfinite_manifold);

    triangulation.refine_global(9 - 2 * dim);
  }

//  @sect3{Setup of data structures}

// 下面的函数在其他教程中是众所周知的，它枚举了自由度，创建了一个约束对象并为线性系统设置了一个稀疏矩阵。唯一值得一提的是，该函数接收了一个映射对象的引用，然后我们将其传递给 VectorTools::interpolate_boundary_values() 函数，以确保我们的边界值在用于装配的高阶网格上被评估。在本例中，这并不重要，因为外表面是平的，但对于弯曲的外单元，这将导致边界值的更精确的近似。

  template <int dim>
  void PoissonProblem<dim>::setup_system(const Mapping<dim> &mapping)
  {
    dof_handler.distribute_dofs(fe);
    std::cout << "   Number of active cells:       "
              << triangulation.n_global_active_cells() << std::endl;
    std::cout << "   Number of degrees of freedom: " << dof_handler.n_dofs()
              << std::endl;

    {
      TimerOutput::Scope scope(timer, "Compute constraints");

      constraints.clear();

      DoFTools::make_hanging_node_constraints(dof_handler, constraints);
      VectorTools::interpolate_boundary_values(
        mapping, dof_handler, 0, ExactSolution<dim>(), constraints);

      constraints.close();
    }

    DynamicSparsityPattern dsp(dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern(dof_handler, dsp, constraints, false);

    sparsity_pattern.copy_from(dsp);
    system_matrix.reinit(sparsity_pattern);

    solution.reinit(dof_handler.n_dofs());
    system_rhs.reinit(dof_handler.n_dofs());
  }
// @sect3{Assembly of the system matrix and right hand side}

// 组装线性系统的函数在前面的教程程序中也是众所周知的。有一点需要注意的是，我们将正交点的数量设置为多项式的度数加2，而不是像其他大多数教程中的度数加1。这是因为我们期望有一些额外的精度，因为映射也涉及到比解的多项式多一度的程度。

// 汇编中唯一有点不寻常的代码是我们计算单元格矩阵的方式。我们没有使用正交点索引、行和矩阵列的三个嵌套循环，而是首先收集形状函数的导数，乘以系数和积分因子`JxW`的乘积的平方根，放在一个单独的矩阵`partial_matrix`中。为了计算单元矩阵，我们在 "partial_matrix.mTmult(cell_matrix, partial_matrix); "一行中执行 "cell_matrix = partial_matrix * transpose(partial_matrix)"。为了理解这一点，我们要知道矩阵与矩阵的乘法是对`partial_matrix`的各列进行求和。如果我们用
// $a(\mathbf{x}_q)$ 表示系数，临时矩阵的条目是 $\sqrt{\text{det}(J) w_q a(x)} \frac{\partial \varphi_i(\boldsymbol
//  \xi_q)}{\partial x_k}$ 。如果我们将该矩阵的第i行与第j列相乘，我们计算出一个涉及 $\sum_q \sum_{k=1}^d \sqrt{\text{det}(J) w_q a(x)} \frac{\partial
//  \varphi_i(\boldsymbol \xi_q)}{\partial x_k} \sqrt{\text{det}(J) w_q a(x)}
//  \frac{\partial \varphi_j(\boldsymbol \xi_q)}{\partial x_k} = \sum_q
//  \sum_{k=1}^d\text{det}(J) w_q a(x)\frac{\partial \varphi_i(\boldsymbol
//  \xi_q)}{\partial x_k} \frac{\partial \varphi_j(\boldsymbol
//  \xi_q)}{\partial x_k}$ 的嵌套和，这正是拉普拉斯方程的双线性形式所需的条款。

// 选择这种有点不寻常的方案的原因是由于计算三维中相对较高的多项式程度的单元矩阵所涉及的繁重工作。由于我们想在这个教程程序中强调映射的成本，我们最好以优化的方式进行装配，以便不追逐已经被社区解决的瓶颈。矩阵-矩阵乘法是HPC背景下最好的优化内核之一， FullMatrix::mTmult() 函数将调用到那些优化的BLAS函数。如果用户在配置deal.II时提供了一个好的BLAS库（如OpenBLAS或英特尔的MKL），那么单元矩阵的计算将执行到接近处理器的峰值算术性能。顺便提一下，尽管有优化的矩阵-矩阵乘法，但目前的策略在复杂性方面是次优的，因为要做的工作与 $(p+1)^9$ 度 $p$ 的运算成正比（这也适用于用FEValues的通常评估）。我们可以通过利用形状函数的张量乘积结构，用 $\mathcal O((p+1)^7)$ 的操作来计算单元格矩阵，就像交易二中的无矩阵框架那样。我们参考 step-37 和张量积感知评估器FEEvaluation的文档，以了解如何实现更有效的单元矩阵计算的细节。

  template <int dim>
  void PoissonProblem<dim>::assemble_system(const Mapping<dim> &mapping)
  {
    TimerOutput::Scope scope(timer, "Assemble linear system");

    const QGauss<dim> quadrature_formula(fe.degree + 2);
    FEValues<dim>     fe_values(mapping,
                            fe,
                            quadrature_formula,
                            update_values | update_gradients |
                              update_quadrature_points | update_JxW_values);

    const unsigned int dofs_per_cell = fe.n_dofs_per_cell();
    const unsigned int n_q_points    = quadrature_formula.size();

    FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
    Vector<double>     cell_rhs(dofs_per_cell);
    FullMatrix<double> partial_matrix(dofs_per_cell, dim * n_q_points);
    std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        cell_rhs = 0.;
        fe_values.reinit(cell);

        for (unsigned int q_index = 0; q_index < n_q_points; ++q_index)
          {
            const double current_coefficient =
              coefficient(fe_values.quadrature_point(q_index));
            for (unsigned int i = 0; i < dofs_per_cell; ++i)
              {
                for (unsigned int d = 0; d < dim; ++d)
                  partial_matrix(i, q_index * dim + d) =
                    std::sqrt(fe_values.JxW(q_index) * current_coefficient) *
                    fe_values.shape_grad(i, q_index)[d];
                cell_rhs(i) +=
                  (fe_values.shape_value(i, q_index) * // phi_i(x_q)
                   (-dim) *                            // f(x_q)
                   fe_values.JxW(q_index));            // dx
              }
          }

        partial_matrix.mTmult(cell_matrix, partial_matrix);

        cell->get_dof_indices(local_dof_indices);
        constraints.distribute_local_to_global(
          cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
      }
  }

//  @sect3{Solution of the linear system}

// 对于线性系统的求解，我们选择一个简单的雅可比条件共轭梯度求解器，类似于早期教程中的设置。

  template <int dim>
  void PoissonProblem<dim>::solve()
  {
    TimerOutput::Scope scope(timer, "Solve linear system");

    SolverControl            solver_control(1000, 1e-12);
    SolverCG<Vector<double>> solver(solver_control);

    PreconditionJacobi<SparseMatrix<double>> preconditioner;
    preconditioner.initialize(system_matrix);

    solver.solve(system_matrix, solution, system_rhs, preconditioner);
    constraints.distribute(solution);

    std::cout << "   Number of solver iterations:  "
              << solver_control.last_step() << std::endl;
  }

//  @sect3{Output of the solution and computation of errors}

// 在下一个函数中，我们对解决方案做了各种后处理步骤，所有这些步骤都以这种或那种方式涉及映射。

// 我们做的第一个操作是把解决方案以及材料ID写到VTU文件中。这与其他许多教程程序中的做法类似。这个教程程序中提出的新内容是，我们要确保写到文件中用于可视化的数据实际上是deal.II内部使用的数据的忠实代表。这是因为大多数可视化数据格式只用顶点坐标表示单元，但没有办法表示deal.II中使用高阶映射时的曲线边界--换句话说，你在可视化工具中看到的东西实际上不是你正在计算的东西。顺带一提，在使用高阶形状函数时也是如此。大多数可视化工具只呈现双线性/三线性的表示。这在 DataOut::build_patches().) 中有详细的讨论。

// 所以我们需要确保高阶表示被写入文件中。我们需要考虑两个特别的话题。首先，我们通过 DataOutBase::VtkFlags 告诉DataOut对象，我们打算把元素的细分解释为高阶拉格朗日多项式，而不是双线性补丁的集合。最近的可视化程序，如ParaView 5.5版或更新的程序，然后可以呈现高阶解决方案（更多细节见wiki
//  page）。其次，我们需要确保映射被传递给 DataOut::build_patches() 方法。最后，DataOut类默认只打印boundary单元的曲面，所以我们需要确保通过映射将内部单元也打印成曲面。

  template <int dim>
  void PoissonProblem<dim>::postprocess(const Mapping<dim> &mapping)
  {
    {
      TimerOutput::Scope scope(timer, "Write output");

      DataOut<dim> data_out;

      DataOutBase::VtkFlags flags;
      flags.write_higher_order_cells = true;
      data_out.set_flags(flags);

      data_out.attach_dof_handler(dof_handler);
      data_out.add_data_vector(solution, "solution");

      Vector<double> material_ids(triangulation.n_active_cells());
      for (const auto &cell : triangulation.active_cell_iterators())
        material_ids[cell->active_cell_index()] = cell->material_id();
      data_out.add_data_vector(material_ids, "material_ids");

      data_out.build_patches(mapping,
                             fe.degree,
                             DataOut<dim>::curved_inner_cells);

      std::ofstream file(
        ("solution-" +
         std::to_string(triangulation.n_global_levels() - 10 + 2 * dim) +
         ".vtu")
          .c_str());

      data_out.write_vtu(file);
    }

// 后处理函数的下一个操作是对照分析解计算 $L_2$ 和 $H^1$ 误差。由于分析解是一个二次多项式，我们期望在这一点上得到一个非常准确的结果。如果我们是在一个具有平面面的简单网格上求解，并且系数的跳动与单元间的面对齐，那么我们会期望数值结果与分析解相吻合，直至舍去精度。然而，由于我们使用的是跟随球体的变形单元，这些单元只能由4度的多项式跟踪（比有限元的度数多一个），我们会发现在 $10^{-7}$ 附近有一个误差。我们可以通过增加多项式的度数或细化网格来获得更多的精度。

    {
      TimerOutput::Scope scope(timer, "Compute error norms");

      Vector<double> norm_per_cell_p(triangulation.n_active_cells());

      VectorTools::integrate_difference(mapping,
                                        dof_handler,
                                        solution,
                                        ExactSolution<dim>(),
                                        norm_per_cell_p,
                                        QGauss<dim>(fe.degree + 2),
                                        VectorTools::L2_norm);
      std::cout << "   L2 error vs exact solution:   "
                << norm_per_cell_p.l2_norm() << std::endl;

      VectorTools::integrate_difference(mapping,
                                        dof_handler,
                                        solution,
                                        ExactSolution<dim>(),
                                        norm_per_cell_p,
                                        QGauss<dim>(fe.degree + 2),
                                        VectorTools::H1_norm);
      std::cout << "   H1 error vs exact solution:   "
                << norm_per_cell_p.l2_norm() << std::endl;
    }

// 我们在这里做的最后一个后处理操作是用KellyErrorEstimator计算出一个误差估计。我们使用了与 step-6 教程程序中完全相同的设置，只是我们还交出了映射，以确保误差是沿着曲线元素评估的，与程序的其余部分一致。然而，我们并没有真正使用这里的结果来驱动网格适应步骤（会沿着球体细化材料界面周围的网格），因为这里的重点是这个操作的成本。

    {
      TimerOutput::Scope scope(timer, "Compute error estimator");

      Vector<float> estimated_error_per_cell(triangulation.n_active_cells());
      KellyErrorEstimator<dim>::estimate(
        mapping,
        dof_handler,
        QGauss<dim - 1>(fe.degree + 1),
        std::map<types::boundary_id, const Function<dim> *>(),
        solution,
        estimated_error_per_cell);
      std::cout << "   Max cell-wise error estimate: "
                << estimated_error_per_cell.linfty_norm() << std::endl;
    }
  }

//  @sect3{The PoissonProblem::run() function}

// 最后，我们定义了`run()`函数，控制我们如何执行这个程序（由main()函数以常规方式调用）。我们首先调用`create_grid()`函数，用适当的流形设置我们的几何体。然后我们运行两个求解器链的实例，从方程的设置开始，组装线性系统，用一个简单的迭代求解器求解，以及上面讨论的后处理。这两个实例在使用映射的方式上有所不同。第一个使用传统的MappingQGeneric映射对象，我们将其初始化为比有限元多一级的程度；毕竟，我们期望几何表示是瓶颈，因为分析解只是二次多项式。实际上，事情在相当程度上是相互关联的，因为实坐标中多项式的评估涉及到高阶多项式的映射，而高阶多项式代表一些光滑的有理函数。因此，高阶多项式还是有回报的，所以进一步增加映射的度数是没有意义的)。一旦第一遍完成，我们就让定时器打印出各个阶段的计算时间的摘要。

  template <int dim>
  void PoissonProblem<dim>::run()
  {
    create_grid();

    {
      std::cout << std::endl
                << "====== Running with the basic MappingQGeneric class ====== "
                << std::endl
                << std::endl;

      MappingQGeneric<dim> mapping(fe.degree + 1);
      setup_system(mapping);
      assemble_system(mapping);
      solve();
      postprocess(mapping);

      timer.print_summary();
      timer.reset();
    }

// 对于第二个实例，我们转而设置了MappingQCache类。它的使用非常简单。在构建好它之后（考虑到我们希望它在其他情况下显示正确的度数功能，所以用度数），我们通过 MappingQCache::initialize() 函数填充缓存。在这个阶段，我们为缓存指定我们想要使用的映射（很明显，与之前的MappingQGeneric相同，以便重复相同的计算），然后再次运行相同的函数，现在交出修改后的映射。最后，我们再次打印重置后的累计壁挂时间，看看这些时间与原来的设置相比如何。

    {
      std::cout
        << "====== Running with the optimized MappingQCache class ====== "
        << std::endl
        << std::endl;

      MappingQCache<dim> mapping(fe.degree + 1);
      {
        TimerOutput::Scope scope(timer, "Initialize mapping cache");
        mapping.initialize(MappingQGeneric<dim>(fe.degree + 1), triangulation);
      }
      std::cout << "   Memory consumption cache:     "
                << 1e-6 * mapping.memory_consumption() << " MB" << std::endl;

      setup_system(mapping);
      assemble_system(mapping);
      solve();
      postprocess(mapping);

      timer.print_summary();
    }
  }
} // namespace Step65

int main()
{
  Step65::PoissonProblem<3> test_program;
  test_program.run();
  return 0;
}


//! \file
// e_float_test.cpp

// Copyright Paul A. Bristow 2011.
// Copyright Christopher Kormanyos 2011.

// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

// Test of using e_float and Boost.Check macro/functions.

#define BOOST_TEST_MAIN
#define BOOST_LIB_DIAGNOSTIC "on"// Show library file details.
// Linking to lib file: libboost_unit_test_framework-vc100-mt-gd-1_48.lib  (trunk at Jul 11)

//  #define E_FLOAT_DIGITS10 50 as command line using MSVC or in jamfile (but needs b2 -a option?)

#include <iostream>
using std::cout;
using std::cin;
using std::endl;
using std::dec;
using std::hex;
using std::boolalpha;
using std::scientific;
using std::fixed;
using std::defaultfloat;
using std::showpos;
using std::showpoint;

#include <iomanip>
using std::setprecision;
using std::setw;
#include <string>
using std::string;
#include <sstream>
//using std::istringstream;
//using std::ostringstream

#include <boost/test/unit_test.hpp> // Enhanced for unit_test framework autolink.
#include <boost/test/floating_point_comparison.hpp>

#include <boost/e_float/e_float.hpp>
#include <boost/e_float/e_float_constants.hpp>

namespace test
{
  namespace real
  {
    // See \boost-sandbox\e_float\libs\e_float\test\real\test_real.cpp  and /cases/ *.cpp
    // \boost-sandbox\e_float\libs\e_float\test\real\cases\test_case_0000x_overflow_underflow.cpp
    bool test_case_00001_overflow_mul_x(const bool);
    bool test_case_00002_underflow_mul_x (const bool);
    bool test_case_00003_overflow_x_mul_by_n(const bool);
    bool test_case_00004_underflow_x_div_by_n(const bool);
    // \boost-sandbox\e_float\libs\e_float\test\real\cases\test_case_0000y_write_to_ostream.cpp
    bool test_case_00006_write_os_floatfield_fixed(const bool);
    bool test_case_00007_write_os_floatfield_scientific(const bool);
    bool test_case_00008_write_os_floatfield_not_set(const bool);
    // \boost-sandbox\e_float\libs\e_float\test\real\cases\test_case_0000z_global_ops_pod.cpp
    bool test_case_00009_global_ops_pod_equate         (const bool);
    bool test_case_00010_global_ops_pod_operations     (const bool);
    // \boost-sandbox\e_float\libs\e_float\test\real\cases\test_case_00011_various_elem_math.cpp
    bool test_case_00011_various_elem_math             (const bool);
    bool test_case_00021_bernoulli                     (const bool);
    bool test_case_00051_factorial                     (const bool);
    bool test_case_00052_factorial2                    (const bool);
    bool test_case_00071_various_int_func              (const bool);
    bool test_case_00101_sin                           (const bool);
    bool test_case_00102_cos                           (const bool);
    bool test_case_00103_exp                           (const bool);
    bool test_case_00104_log                           (const bool);
    bool test_case_00105_sqrt                          (const bool);
    bool test_case_00106_rootn                         (const bool);
    bool test_case_00111_sin_small_x                   (const bool);
    bool test_case_00112_cos_x_near_pi_half            (const bool);
    bool test_case_00113_atan_x_small_to_large         (const bool);
    bool test_case_00114_various_trig                  (const bool);
    bool test_case_00115_various_elem_trans            (const bool);
    bool test_case_00121_sinh                          (const bool);
    bool test_case_00122_cosh                          (const bool);
    bool test_case_00123_tanh                          (const bool);
    bool test_case_00124_asinh                         (const bool);
    bool test_case_00125_acosh                         (const bool);
    bool test_case_00126_atanh                         (const bool);
    bool test_case_00201_gamma                         (const bool);
    bool test_case_00202_gamma_medium_x                (const bool);
    bool test_case_00203_gamma_small_x                 (const bool);
    bool test_case_00204_gamma_tiny_x                  (const bool);
    bool test_case_00205_gamma_near_neg_n              (const bool);
    bool test_case_00221_various_gamma_func            (const bool);
    bool test_case_00901_zeta_small_x                  (const bool);
    bool test_case_00902_zeta_all_x                    (const bool);
    bool test_case_00903_zeta_neg_x                    (const bool);
  } // namespace real
} // namespace test

//Macros to Check using manips output expected string result, for example:
// CHECK_OUT(hex << showbase << setw(10) << i, "       0xf")
// CHECK_OUT(scientific << setw(20) << d, "       1.234568e+001");

// Compare a output with expected result.
#define CHECK_OUT(manips, result)\
{ \
  std::ostringstream oss;\
  oss << manips;\
  BOOST_CHECK_EQUAL(oss.str(), result);\
}

#define  CHECK_LENGTH(manips, result)\
{\
  BOOST_CHECK_EQUAL(oss.str().length(), strlen(result));\
}\

// Compare results of reading string in,
#define  CHECK_IN(in, value)\
{\
  e_float r;\
  std::istringstream iss(in);\
  iss >> r;\
  BOOST_CHECK_CLOSE_FRACTION(r, value, std::numeric_limits<e_float>::epsilon());\
} // #define  CHECK_IN(in, value, sd, df, types)

// CHECK_OUT_IN Output via manips, and read back in, check is same. 'Loopback'.
#define  CHECK_OUT_IN(manips, result, value)\
{\
  std::stringstream ss;\
  ss << manips;\
  BOOST_CHECK_EQUAL(ss.str(), result);\
  e_float r;\
  ss >> r;\
  BOOST_CHECK_CLOSE_FRACTION(r, value, std::numeric_limits<e_float>::epsilon());\
}// #define  CHECK_OUT_IN(manips, result)

// Must #define E_FLOAT_TYPE_EFX; in project properties.

// Note: exact double means exactly representable as double, for example: 0.5,
// but NOT 0.1.
// Integral may be an integer or a double having an integral value.

BOOST_AUTO_TEST_CASE(e_float_test_template)
{ // These are just examples of using Boost.Test.
  // Need to be removed when no longer helpful.
  BOOST_TEST_MESSAGE("Test Boost.e_float"); // Only appears if command line has --log_level="message"

  string m = "Test with ";
  m+= __FILE__;
  m+= " edited ";
  m += __TIMESTAMP__ ".\n";
  BOOST_TEST_MESSAGE(m);
  BOOST_CHECK(true);
  BOOST_CHECK_EQUAL(1, 1);
  BOOST_CHECK_NE(1, -1);
  BOOST_CHECK_CLOSE_FRACTION(1.0, 1.0+std::numeric_limits<double>::epsilon(), std::numeric_limits<double>::epsilon());
  double d = 123.456789;
  // cout << scientific << d << endl; //  Outputs: "1.234568e+003"
  CHECK_OUT(d, "123.457"); // Default. == << std::defaultfloat
  CHECK_OUT(defaultfloat << d, "123.457"); // Default. == << std::defaultfloat
  string ddef = "123.457"; // default float output.
  CHECK_OUT(d, ddef); // Default. == << std::defaultfloat
  CHECK_OUT(defaultfloat << d, ddef); // Default. == << std::defaultfloat
  CHECK_OUT(scientific << d, "1.234568e+002");
  CHECK_OUT(fixed << d, "123.456789");
  int m1 = -1; // negative variable.
  CHECK_OUT(m1, "-1"); //  negative constant.
  CHECK_OUT(hex << m1, "ffffffff"); // with hex manipulator.
  // Checking input with inline.
  e_float r;
  std::istringstream iss("123.456");
  iss >> r;
  BOOST_CHECK_CLOSE_FRACTION(r, e_float("123.456"), std::numeric_limits<e_float>::epsilon());
  BOOST_CHECK_EQUAL(r, e_float("123.456")); // Also works
  // Repeat same test using CHECK_IN macro defined above.
  CHECK_IN("123.456", e_float("123.456"));

  //CHECK_IN("123.456", e_float(123.456)); // Mistaken conversion from less accurate double.
  // Fails r{123.456} and e_float(123.456){123.4560000000000030695446184836328029632568359375}
  // differ by more than 1e-49.

  // CHECK_OUT_IN Output via manips, and read back in, check is same. 'Loopback'.
  // #define  CHECK_OUT_IN(manips, result, value)
  { // Integer example.
    int i = 255;
    std::string result = "ff";
    int value = 255;
    std::stringstream ss;
    ss << hex << i;
    BOOST_CHECK_EQUAL(ss.str(), result);
    int read;
    ss >> read;
    BOOST_CHECK_EQUAL(read, value);
  }
  { // double example
    double w = 123.456;
    std::string result = "123.456000";
    double value = 123.456;
    std::stringstream ss;
    ss << std::fixed << w;
    //cout << ss.str() << endl;
    BOOST_CHECK_EQUAL(ss.str(), result);
    double read;
    ss >> read;
    BOOST_CHECK_EQUAL(read, value);
    BOOST_CHECK_CLOSE_FRACTION(read, value, std::numeric_limits<double>::epsilon());
  }
  { // e_float example
    e_float w("123.456");
    std::string result = "123.456000";  // double result was "123.456000";
    e_float value("123.456");
    std::stringstream ss;
    ss << std::fixed << w;
    //cout << ss.str() << endl;
    BOOST_CHECK_EQUAL(ss.str(), result);
    e_float read;
    ss >> read;
    BOOST_CHECK_EQUAL(read, value);
    BOOST_CHECK_CLOSE_FRACTION(read, value, std::numeric_limits<e_float>::epsilon());

    CHECK_OUT_IN(std::fixed << w, result, value);
  }

} // BOOST_AUTO_TEST_CASE(e_float_template)

BOOST_AUTO_TEST_CASE(e_float_test_macros)
{ // Check some macro values.
  //
  BOOST_CHECK_EQUAL(E_FLOAT_DIGITS10, 50); // Assumes we are testing at 50 digits, NOT the default.
} // BOOST_AUTO_TEST_CASE(e_float_test_macros)

BOOST_AUTO_TEST_CASE(e_float_test_ios)
{ // Check some IOS defaults.
  BOOST_CHECK_EQUAL(cout.precision(), 6);
  std::ostringstream oss;
  BOOST_CHECK_EQUAL(oss.precision(), 6);
  }

BOOST_AUTO_TEST_CASE(e_float_test_input)
{ //
  BOOST_TEST_MESSAGE("Test Boost.e_float input.");

   CHECK_IN("2", e_float("2"));
   CHECK_IN("-2", e_float("-2"));
   CHECK_IN("+2", e_float("+2"));
   CHECK_IN("-2.", e_float("-2."));

   CHECK_IN("123.456", e_float("123.456"));
   CHECK_IN("0.0123456", e_float("0.0123456"));
   CHECK_IN("1e-6", e_float("1e-6"));
   CHECK_IN("-1e-6", e_float("-1e-6"));
} // BOOST_AUTO_TEST_CASE(e_float_test_input)

BOOST_AUTO_TEST_CASE(e_float_tests)
{ // Original real tests from \e_float\libs\e_float\test\real\cases
  BOOST_TEST_MESSAGE("Test Boost.e_float numerical.");

  BOOST_CHECK(test::real::test_case_00001_overflow_mul_x                 (false));
  BOOST_CHECK(test::real::test_case_00002_underflow_mul_x                (false));
  BOOST_CHECK(test::real::test_case_00003_overflow_x_mul_by_n            (false));
  BOOST_CHECK(test::real::test_case_00004_underflow_x_div_by_n           (false));
  BOOST_CHECK(test::real::test_case_00006_write_os_floatfield_fixed      (false));
  BOOST_CHECK(test::real::test_case_00007_write_os_floatfield_scientific (false));
  BOOST_CHECK(test::real::test_case_00008_write_os_floatfield_not_set    (false));
  BOOST_CHECK( test::real::test_case_00001_overflow_mul_x                (false));
  BOOST_CHECK( test::real::test_case_00002_underflow_mul_x               (false));
  BOOST_CHECK( test::real::test_case_00003_overflow_x_mul_by_n           (false));
  BOOST_CHECK( test::real::test_case_00004_underflow_x_div_by_n          (false));
  BOOST_CHECK( test::real::test_case_00006_write_os_floatfield_fixed     (false));
  BOOST_CHECK( test::real::test_case_00007_write_os_floatfield_scientific(false));
  BOOST_CHECK( test::real::test_case_00008_write_os_floatfield_not_set   (false));
  BOOST_CHECK( test::real::test_case_00009_global_ops_pod_equate         (false));
  BOOST_CHECK( test::real::test_case_00010_global_ops_pod_operations     (false));
  BOOST_CHECK( test::real::test_case_00011_various_elem_math             (false));
  BOOST_CHECK( test::real::test_case_00021_bernoulli                     (false));
  BOOST_CHECK( test::real::test_case_00051_factorial                     (false));
  BOOST_CHECK( test::real::test_case_00052_factorial2                    (false));
  BOOST_CHECK( test::real::test_case_00071_various_int_func              (false));
  BOOST_CHECK( test::real::test_case_00101_sin                           (false));
  BOOST_CHECK( test::real::test_case_00102_cos                           (false));
  BOOST_CHECK( test::real::test_case_00103_exp                           (false));
  BOOST_CHECK( test::real::test_case_00104_log                           (false));
  BOOST_CHECK( test::real::test_case_00105_sqrt                          (false));
  BOOST_CHECK( test::real::test_case_00106_rootn                         (false));
  BOOST_CHECK( test::real::test_case_00111_sin_small_x                   (false));
  BOOST_CHECK( test::real::test_case_00112_cos_x_near_pi_half            (false));
  BOOST_CHECK( test::real::test_case_00113_atan_x_small_to_large         (false));
  BOOST_CHECK( test::real::test_case_00114_various_trig                  (false));
  BOOST_CHECK( test::real::test_case_00115_various_elem_trans            (false));
  BOOST_CHECK( test::real::test_case_00121_sinh                          (false));
  BOOST_CHECK( test::real::test_case_00122_cosh                          (false));
  BOOST_CHECK( test::real::test_case_00123_tanh                          (false));
  BOOST_CHECK( test::real::test_case_00124_asinh                         (false));
  BOOST_CHECK( test::real::test_case_00125_acosh                         (false));
  BOOST_CHECK( test::real::test_case_00126_atanh                         (false));
  BOOST_CHECK( test::real::test_case_00201_gamma                         (false));
  BOOST_CHECK( test::real::test_case_00202_gamma_medium_x                (false));
  BOOST_CHECK( test::real::test_case_00203_gamma_small_x                 (false));
  BOOST_CHECK( test::real::test_case_00204_gamma_tiny_x                  (false));
  BOOST_CHECK( test::real::test_case_00205_gamma_near_neg_n              (false));
  BOOST_CHECK( test::real::test_case_00221_various_gamma_func            (false));
  BOOST_CHECK( test::real::test_case_00901_zeta_small_x                  (false));
  BOOST_CHECK( test::real::test_case_00902_zeta_all_x                    (false));
  BOOST_CHECK( test::real::test_case_00903_zeta_neg_x                    (false));

} //  BOOST_AUTO_TEST_CASE(e_float_tests)


// Copyright (c) 2020 Chris Richardson
// FEniCS Project
// SPDX-License-Identifier:    MIT

#include "polyset.h"
#include "cell.h"
#include "indexing.h"
#include <Eigen/Dense>
#include <array>
#include <cmath>

using namespace basix;

namespace
{
// Compute coefficients in the Jacobi Polynomial recurrence relation
constexpr std::array<double, 3> jrc(int a, int n)
{
  double an = (a + 2 * n + 1) * (a + 2 * n + 2)
              / static_cast<double>(2 * (n + 1) * (a + n + 1));
  double bn = a * a * (a + 2 * n + 1)
              / static_cast<double>(2 * (n + 1) * (a + n + 1) * (a + 2 * n));
  double cn = n * (a + n) * (a + 2 * n + 2)
              / static_cast<double>((n + 1) * (a + n + 1) * (a + 2 * n));
  return {an, bn, cn};
}
//-----------------------------------------------------------------------------
// Compute the complete set of derivatives from 0 to nderiv, for all the
// polynomials up to order n on a line segment. The polynomials used are
// Legendre Polynomials, with the recurrence relation given by
// n P(n) = (2n - 1) x P_{n-1} - (n - 1) P_{n-2} in the interval [-1, 1]. The
// range is rescaled here to [0, 1].
std::vector<Eigen::ArrayXXd>
tabulate_polyset_line_derivs(int degree, int nderiv, const Eigen::ArrayXXd& x)
{
  assert(x.cols() == 1);
  const Eigen::ArrayXXd X = x * 2.0 - 1.0;

  const int m = (degree + 1);

  std::vector<Eigen::ArrayXXd> dresult(nderiv + 1);
  for (int k = 0; k < nderiv + 1; ++k)
  {
    // Get reference to this derivative
    Eigen::ArrayXXd result(x.rows(), m);

    if (k == 0)
      result.col(0).fill(1.0);
    else
      result.col(0).setZero();

    for (int p = 1; p < degree + 1; ++p)
    {
      const double a = 1.0 - 1.0 / static_cast<double>(p);
      result.col(p) = X * result.col(p - 1) * (a + 1.0);
      if (k > 0)
        result.col(p) += 2 * k * dresult[k - 1].col(p - 1) * (a + 1.0);
      if (p > 1)
        result.col(p) -= result.col(p - 2) * a;
    }

    dresult[k] = result;
  }

  // Normalise
  for (int k = 0; k < nderiv + 1; ++k)
  {
    for (int p = 0; p < degree + 1; ++p)
      dresult[k].col(p) *= std::sqrt(p + 0.5);
  }

  return dresult;
}
//-----------------------------------------------------------------------------
// Compute the complete set of derivatives from 0 to nderiv, for all the
// polynomials up to order n on a triangle in [0, 1][0, 1].
// The polynomials P_{pq} are built up in sequence, firstly along q = 0, which
// is a line segment, as in tabulate_polyset_interval_derivs above, but with a
// change of variables. The polynomials are then extended in the q direction,
// using the relation given in Sherwin and Karniadakis 1995
// (https://doi.org/10.1016/0045-7825(94)00745-9)
std::vector<Eigen::ArrayXXd>
tabulate_polyset_triangle_derivs(int n, int nderiv, const Eigen::ArrayXXd& pts)

{
  assert(pts.cols() == 2);

  Eigen::ArrayXXd x = pts * 2.0 - 1.0;

  const int m = (n + 1) * (n + 2) / 2;
  const int md = (nderiv + 1) * (nderiv + 2) / 2;
  std::vector<Eigen::ArrayXXd> dresult(md);

  // f3 = ((1-y)/2)^2
  const Eigen::ArrayXd f3 = (1.0 - x.col(1)).square() * 0.25;

  // Iterate over derivatives in increasing order, since higher derivatives
  // depend on earlier calculations
  Eigen::ArrayXXd result(pts.rows(), m);
  for (int k = 0; k < nderiv + 1; ++k)
  {
    for (int kx = 0; kx < k + 1; ++kx)
    {
      const int ky = k - kx;

      if (kx == 0 and ky == 0)
        result.col(0).fill(1.0);
      else
        result.col(0).setZero();

      for (int p = 1; p < n + 1; ++p)
      {
        const double a
            = static_cast<double>(2 * p - 1) / static_cast<double>(p);
        result.col(idx(p, 0))
            = (x.col(0) + 0.5 * x.col(1) + 0.5) * result.col(idx(p - 1, 0)) * a;
        if (kx > 0)
        {
          result.col(idx(p, 0))
              += 2 * kx * a * dresult[idx(kx - 1, ky)].col(idx(p - 1, 0));
        }

        if (ky > 0)
        {
          result.col(idx(p, 0))
              += ky * a * dresult[idx(kx, ky - 1)].col(idx(p - 1, 0));
        }

        if (p > 1)
        {
          // y^2 terms
          result.col(idx(p, 0)) -= f3 * result.col(idx(p - 2, 0)) * (a - 1.0);

          if (ky > 0)
          {
            result.col(idx(p, 0))
                -= ky * (x.col(1) - 1.0)
                   * dresult[idx(kx, ky - 1)].col(idx(p - 2, 0)) * (a - 1.0);
          }

          if (ky > 1)
          {
            result.col(idx(p, 0))
                -= ky * (ky - 1) * dresult[idx(kx, ky - 2)].col(idx(p - 2, 0))
                   * (a - 1.0);
          }
        }
      }

      for (int p = 0; p < n; ++p)
      {
        result.col(idx(p, 1))
            = result.col(idx(p, 0)) * (x.col(1) * (1.5 + p) + 0.5 + p);
        if (ky > 0)
        {
          result.col(idx(p, 1))
              += 2 * ky * (1.5 + p) * dresult[idx(kx, ky - 1)].col(idx(p, 0));
        }

        for (int q = 1; q < n - p; ++q)
        {
          const auto [a1, a2, a3] = jrc(2 * p + 1, q);
          result.col(idx(p, q + 1))
              = result.col(idx(p, q)) * (x.col(1) * a1 + a2)
                - result.col(idx(p, q - 1)) * a3;
          if (ky > 0)
          {
            result.col(idx(p, q + 1))
                += 2 * ky * a1 * dresult[idx(kx, ky - 1)].col(idx(p, q));
          }
        }
      }

      // Store this derivative
      dresult[idx(kx, ky)] = result;
    }
  }

  // Normalisation
  for (std::size_t j = 0; j < dresult.size(); ++j)
  {
    for (int p = 0; p < n + 1; ++p)
      for (int q = 0; q < n - p + 1; ++q)
        dresult[j].col(idx(p, q)) *= std::sqrt((p + 0.5) * (p + q + 1));
  }

  return dresult;
}
//-----------------------------------------------------------------------------
std::vector<Eigen::ArrayXXd>
tabulate_polyset_tetrahedron_derivs(int n, int nderiv,
                                    const Eigen::ArrayXXd& pts)
{
  assert(pts.cols() == 3);

  Eigen::ArrayXXd x = pts * 2.0 - 1.0;

  const int m = (n + 1) * (n + 2) * (n + 3) / 6;
  const int md = (nderiv + 1) * (nderiv + 2) * (nderiv + 3) / 6;
  std::vector<Eigen::ArrayXXd> dresult(md);

  const Eigen::ArrayXd f2 = (x.col(1) + x.col(2)).square() * 0.25;
  const Eigen::ArrayXd f3 = (1.0 + x.col(1) * 2.0 + x.col(2)) * 0.5;
  const Eigen::ArrayXd f4 = (1.0 - x.col(2)) * 0.5;
  const Eigen::ArrayXd f5 = f4 * f4;

  // Traverse derivatives in increasing order
  Eigen::ArrayXXd result(pts.rows(), m);
  for (int k = 0; k < nderiv + 1; ++k)
  {
    for (int j = 0; j < k + 1; ++j)
    {
      for (int kx = 0; kx < j + 1; ++kx)
      {
        const int ky = j - kx;
        const int kz = k - j;
        if (kx == 0 and ky == 0 and kz == 0)
          result.col(0).fill(1.0);
        else
          result.col(0).setZero();

        for (int p = 1; p < n + 1; ++p)
        {
          double a = static_cast<double>(2 * p - 1) / static_cast<double>(p);
          result.col(idx(p, 0, 0))
              = (x.col(0) + 0.5 * (x.col(1) + x.col(2)) + 1.0)
                * result.col(idx(p - 1, 0, 0)) * a;
          if (kx > 0)
          {
            result.col(idx(p, 0, 0))
                += 2 * kx * a
                   * dresult[idx(kx - 1, ky, kz)].col(idx(p - 1, 0, 0));
          }

          if (ky > 0)
          {
            result.col(idx(p, 0, 0))
                += ky * a * dresult[idx(kx, ky - 1, kz)].col(idx(p - 1, 0, 0));
          }

          if (kz > 0)
          {
            result.col(idx(p, 0, 0))
                += kz * a * dresult[idx(kx, ky, kz - 1)].col(idx(p - 1, 0, 0));
          }

          if (p > 1)
          {
            result.col(idx(p, 0, 0))
                -= f2 * result.col(idx(p - 2, 0, 0)) * (a - 1.0);
            if (ky > 0)
            {
              result.col(idx(p, 0, 0))
                  -= ky * (x.col(1) + x.col(2))
                     * dresult[idx(kx, ky - 1, kz)].col(idx(p - 2, 0, 0))
                     * (a - 1.0);
            }

            if (ky > 1)
            {
              result.col(idx(p, 0, 0))
                  -= ky * (ky - 1)
                     * dresult[idx(kx, ky - 2, kz)].col(idx(p - 2, 0, 0))
                     * (a - 1.0);
            }

            if (kz > 0)
            {
              result.col(idx(p, 0, 0))
                  -= kz * (x.col(1) + x.col(2))
                     * dresult[idx(kx, ky, kz - 1)].col(idx(p - 2, 0, 0))
                     * (a - 1.0);
            }

            if (kz > 1)
            {
              result.col(idx(p, 0, 0))
                  -= kz * (kz - 1)
                     * dresult[idx(kx, ky, kz - 2)].col(idx(p - 2, 0, 0))
                     * (a - 1.0);
            }

            if (ky > 0 and kz > 0)
            {
              result.col(idx(p, 0, 0))
                  -= 2.0 * ky * kz
                     * dresult[idx(kx, ky - 1, kz - 1)].col(idx(p - 2, 0, 0))
                     * (a - 1.0);
            }
          }
        }

        for (int p = 0; p < n; ++p)
        {
          result.col(idx(p, 1, 0))
              = result.col(idx(p, 0, 0))
                * ((1.0 + x.col(1)) * p
                   + (2.0 + x.col(1) * 3.0 + x.col(2)) * 0.5);
          if (ky > 0)
          {
            result.col(idx(p, 1, 0))
                += 2 * ky * dresult[idx(kx, ky - 1, kz)].col(idx(p, 0, 0))
                   * (1.5 + p);
          }

          if (kz > 0)
          {
            result.col(idx(p, 1, 0))
                += kz * dresult[idx(kx, ky, kz - 1)].col(idx(p, 0, 0));
          }

          for (int q = 1; q < n - p; ++q)
          {
            auto [aq, bq, cq] = jrc(2 * p + 1, q);
            result.col(idx(p, q + 1, 0))
                = result.col(idx(p, q, 0)) * (f3 * aq + f4 * bq)
                  - result.col(idx(p, q - 1, 0)) * f5 * cq;

            if (ky > 0)
            {
              result.col(idx(p, q + 1, 0))
                  += 2 * ky * dresult[idx(kx, ky - 1, kz)].col(idx(p, q, 0))
                     * aq;
            }

            if (kz > 0)
            {
              result.col(idx(p, q + 1, 0))
                  += kz * dresult[idx(kx, ky, kz - 1)].col(idx(p, q, 0))
                         * (aq - bq)
                     + kz * (1.0 - x.col(2))
                           * dresult[idx(kx, ky, kz - 1)].col(idx(p, q - 1, 0))
                           * cq;
            }

            if (kz > 1)
            {
              // Quadratic term in z
              result.col(idx(p, q + 1, 0))
                  -= kz * (kz - 1)
                     * dresult[idx(kx, ky, kz - 2)].col(idx(p, q - 1, 0)) * cq;
            }
          }
        }

        for (int p = 0; p < n; ++p)
        {
          for (int q = 0; q < n - p; ++q)
          {
            result.col(idx(p, q, 1))
                = result.col(idx(p, q, 0))
                  * ((1.0 + p + q) + x.col(2) * (2.0 + p + q));
            if (kz > 0)
            {
              result.col(idx(p, q, 1))
                  += 2 * kz * (2.0 + p + q)
                     * dresult[idx(kx, ky, kz - 1)].col(idx(p, q, 0));
            }
          }
        }

        for (int p = 0; p < n - 1; ++p)
        {
          for (int q = 0; q < n - p - 1; ++q)
          {
            for (int r = 1; r < n - p - q; ++r)
            {
              auto [ar, br, cr] = jrc(2 * p + 2 * q + 2, r);
              result.col(idx(p, q, r + 1))
                  = result.col(idx(p, q, r)) * (x.col(2) * ar + br)
                    - result.col(idx(p, q, r - 1)) * cr;
              if (kz > 0)
              {
                result.col(idx(p, q, r + 1))
                    += 2 * kz * ar
                       * dresult[idx(kx, ky, kz - 1)].col(idx(p, q, r));
              }
            }
          }
        }

        // Store this derivative
        dresult[idx(kx, ky, kz)] = result;
      }
    }
  }

  for (Eigen::ArrayXXd& result : dresult)
  {
    for (int p = 0; p < n + 1; ++p)
    {
      for (int q = 0; q < n - p + 1; ++q)
      {
        for (int r = 0; r < n - p - q + 1; ++r)
        {
          result.col(idx(p, q, r))
              *= std::sqrt((p + 0.5) * (p + q + 1.0) * (p + q + r + 1.5));
        }
      }
    }
  }

  return dresult;
}
//-----------------------------------------------------------------------------
std::vector<Eigen::ArrayXXd>
tabulate_polyset_pyramid_derivs(int n, int nderiv, const Eigen::ArrayXXd& pts)
{
  assert(pts.cols() == 3);

  Eigen::ArrayXXd x = pts * 2.0 - 1.0;

  const int m = (n + 1) * (n + 2) * (2 * n + 3) / 6;
  const int md = (nderiv + 1) * (nderiv + 2) * (nderiv + 3) / 6;
  std::vector<Eigen::ArrayXXd> dresult(md);

  // Indexing for pyramidal basis functions
  auto pyr_idx = [&n](int p, int q, int r) -> int {
    const int rv = n - r + 1;
    const int r0 = r * (n + 1) * (n - r + 2) + (2 * r - 1) * (r - 1) * r / 6;
    return r0 + p * rv + q;
  };

  const Eigen::ArrayXd f2 = (1.0 - x.col(2)).square() * 0.25;

  // Traverse derivatives in increasing order
  Eigen::ArrayXXd result(pts.rows(), m);
  for (int k = 0; k < nderiv + 1; ++k)
  {
    for (int j = 0; j < k + 1; ++j)
    {
      for (int kx = 0; kx < j + 1; ++kx)
      {
        const int ky = j - kx;
        const int kz = k - j;
        result.setZero();

        const int pyramidal_index = pyr_idx(0, 0, 0);
        assert(pyramidal_index < m);
        if (kx == 0 and ky == 0 and kz == 0)
          result.col(pyramidal_index).fill(1.0);
        else
          result.col(pyramidal_index).setZero();

        // r = 0
        for (int p = 0; p < n + 1; ++p)
        {
          if (p > 0)
          {
            const double a
                = static_cast<double>(p - 1) / static_cast<double>(p);
            result.col(pyr_idx(p, 0, 0)) = (0.5 + x.col(0) + x.col(2) * 0.5)
                                           * result.col(pyr_idx(p - 1, 0, 0))
                                           * (a + 1.0);
            if (kx > 0)
            {
              result.col(pyr_idx(p, 0, 0))
                  += 2.0 * kx
                     * dresult[idx(kx - 1, ky, kz)].col(pyr_idx(p - 1, 0, 0))
                     * (a + 1.0);
            }

            if (kz > 0)
            {
              result.col(pyr_idx(p, 0, 0))
                  += kz * dresult[idx(kx, ky, kz - 1)].col(pyr_idx(p - 1, 0, 0))
                     * (a + 1.0);
            }

            if (p > 1)
            {
              result.col(pyr_idx(p, 0, 0))
                  -= f2 * result.col(pyr_idx(p - 2, 0, 0)) * a;

              if (kz > 0)
              {
                result.col(pyr_idx(p, 0, 0))
                    += kz * (1.0 - x.col(2))
                       * dresult[idx(kx, ky, kz - 1)].col(pyr_idx(p - 2, 0, 0))
                       * a;
              }

              if (kz > 1)
              {
                // quadratic term in z
                result.col(pyr_idx(p, 0, 0))
                    -= kz * (kz - 1)
                       * dresult[idx(kx, ky, kz - 2)].col(pyr_idx(p - 2, 0, 0))
                       * a;
              }
            }
          }

          for (int q = 1; q < n + 1; ++q)
          {
            const double a
                = static_cast<double>(q - 1) / static_cast<double>(q);
            result.col(pyr_idx(p, q, 0)) = (0.5 + x.col(1) + x.col(2) * 0.5)
                                           * result.col(pyr_idx(p, q - 1, 0))
                                           * (a + 1.0);
            if (ky > 0)
            {
              result.col(pyr_idx(p, q, 0))
                  += 2.0 * ky
                     * dresult[idx(kx, ky - 1, kz)].col(pyr_idx(p, q - 1, 0))
                     * (a + 1.0);
            }

            if (kz > 0)
            {
              result.col(pyr_idx(p, q, 0))
                  += kz * dresult[idx(kx, ky, kz - 1)].col(pyr_idx(p, q - 1, 0))
                     * (a + 1.0);
            }

            if (q > 1)
            {
              result.col(pyr_idx(p, q, 0))
                  -= f2 * result.col(pyr_idx(p, q - 2, 0)) * a;

              if (kz > 0)
              {
                result.col(pyr_idx(p, q, 0))
                    += kz * (1.0 - x.col(2))
                       * dresult[idx(kx, ky, kz - 1)].col(pyr_idx(p, q - 2, 0))
                       * a;
              }

              if (kz > 1)
              {
                result.col(pyr_idx(p, q, 0))
                    -= kz * (kz - 1)
                       * dresult[idx(kx, ky, kz - 2)].col(pyr_idx(p, q - 2, 0))
                       * a;
              }
            }
          }
        }

        // Extend into r > 0
        for (int p = 0; p < n; ++p)
        {
          for (int q = 0; q < n; ++q)
          {
            result.col(pyr_idx(p, q, 1))
                = result.col(pyr_idx(p, q, 0))
                  * ((1.0 + p + q) + x.col(2) * (2.0 + p + q));
            if (kz > 0)
            {
              result.col(pyr_idx(p, q, 1))
                  += 2 * kz * dresult[idx(kx, ky, kz - 1)].col(pyr_idx(p, q, 0))
                     * (2.0 + p + q);
            }
          }
        }

        for (int r = 1; r < n + 1; ++r)
        {
          for (int p = 0; p < n - r; ++p)
          {
            for (int q = 0; q < n - r; ++q)
            {
              auto [ar, br, cr] = jrc(2 * p + 2 * q + 2, r);
              result.col(pyr_idx(p, q, r + 1))
                  = result.col(pyr_idx(p, q, r)) * (x.col(2) * ar + br)
                    - result.col(pyr_idx(p, q, r - 1)) * cr;
              if (kz > 0)
              {
                result.col(pyr_idx(p, q, r + 1))
                    += ar * 2 * kz
                       * dresult[idx(kx, ky, kz - 1)].col(pyr_idx(p, q, r));
              }
            }
          }
        }

        dresult[idx(kx, ky, kz)] = result;
      }
    }
  }

  for (Eigen::ArrayXXd& result : dresult)
  {
    for (int r = 0; r < n + 1; ++r)
    {
      for (int p = 0; p < n - r + 1; ++p)

      {
        for (int q = 0; q < n - r + 1; ++q)
        {
          result.col(pyr_idx(p, q, r))
              *= std::sqrt((q + 0.5) * (p + 0.5) * (p + q + r + 1.5));
        }
      }
    }
  }

  return dresult;
}
//-----------------------------------------------------------------------------
std::vector<Eigen::ArrayXXd>
tabulate_polyset_quad_derivs(int n, int nderiv, const Eigen::ArrayXXd& pts)
{
  assert(pts.cols() == 2);
  const int m = (n + 1) * (n + 1);
  const int md = (nderiv + 1) * (nderiv + 2) / 2;

  std::vector<Eigen::ArrayXXd> dresult(md);
  std::vector<Eigen::ArrayXXd> px
      = tabulate_polyset_line_derivs(n, nderiv, pts.col(0));
  std::vector<Eigen::ArrayXXd> py
      = tabulate_polyset_line_derivs(n, nderiv, pts.col(1));

  Eigen::ArrayXXd result(pts.rows(), m);
  for (int kx = 0; kx < nderiv + 1; ++kx)
  {
    for (int ky = 0; ky < nderiv + 1 - kx; ++ky)
    {
      int c = 0;
      for (int i = 0; i < px[kx].cols(); ++i)
        for (int j = 0; j < py[ky].cols(); ++j)
          result.col(c++) = px[kx].col(i) * py[ky].col(j);
      dresult[idx(kx, ky)] = result;
    }
  }

  return dresult;
}
//-----------------------------------------------------------------------------
std::vector<Eigen::ArrayXXd>
tabulate_polyset_hex_derivs(int n, int nderiv, const Eigen::ArrayXXd& pts)
{
  assert(pts.cols() == 3);
  const int m = (n + 1) * (n + 1) * (n + 1);
  const int md = (nderiv + 1) * (nderiv + 2) * (nderiv + 3) / 6;

  std::vector<Eigen::ArrayXXd> px
      = tabulate_polyset_line_derivs(n, nderiv, pts.col(0));
  std::vector<Eigen::ArrayXXd> py
      = tabulate_polyset_line_derivs(n, nderiv, pts.col(1));
  std::vector<Eigen::ArrayXXd> pz
      = tabulate_polyset_line_derivs(n, nderiv, pts.col(2));

  std::vector<Eigen::ArrayXXd> dresult(md);
  Eigen::ArrayXXd result(pts.rows(), m);
  for (int kx = 0; kx < nderiv + 1; ++kx)
  {
    for (int ky = 0; ky < nderiv + 1 - kx; ++ky)
    {
      for (int kz = 0; kz < nderiv + 1 - kx - ky; ++kz)
      {
        int c = 0;
        for (int i = 0; i < px[kx].cols(); ++i)
          for (int j = 0; j < py[ky].cols(); ++j)
            for (int k = 0; k < pz[kz].cols(); ++k)
              result.col(c++) = px[kx].col(i) * py[ky].col(j) * pz[kz].col(k);

        dresult[idx(kx, ky, kz)] = result;
      }
    }
  }

  return dresult;
}
//-----------------------------------------------------------------------------
std::vector<Eigen::ArrayXXd>
tabulate_polyset_prism_derivs(int n, int nderiv, const Eigen::ArrayXXd& pts)
{
  assert(pts.cols() == 3);
  const int m = (n + 1) * (n + 1) * (n + 2) / 2;
  const int md = (nderiv + 1) * (nderiv + 2) * (nderiv + 3) / 6;

  std::vector<Eigen::ArrayXXd> pxy
      = tabulate_polyset_triangle_derivs(n, nderiv, pts.leftCols(2));
  std::vector<Eigen::ArrayXXd> pz
      = tabulate_polyset_line_derivs(n, nderiv, pts.col(2));

  std::vector<Eigen::ArrayXXd> dresult(md);
  Eigen::ArrayXXd result(pts.rows(), m);
  for (int kx = 0; kx < nderiv + 1; ++kx)
  {
    for (int ky = 0; ky < nderiv + 1 - kx; ++ky)
    {
      for (int kz = 0; kz < nderiv + 1 - kx - ky; ++kz)
      {
        int c = 0;
        for (int i = 0; i < pxy[idx(kx, ky)].cols(); ++i)
          for (int k = 0; k < pz[kz].cols(); ++k)
            result.col(c++) = pxy[idx(kx, ky)].col(i) * pz[kz].col(k);

        dresult[idx(kx, ky, kz)] = result;
      }
    }
  }

  return dresult;
}
} // namespace
//-----------------------------------------------------------------------------
std::vector<Eigen::ArrayXXd> polyset::tabulate(cell::type celltype, int n,
                                               int nderiv,
                                               const Eigen::ArrayXXd& pts)
{
  switch (celltype)
  {
  case cell::type::interval:
    return tabulate_polyset_line_derivs(n, nderiv, pts);
  case cell::type::triangle:
    return tabulate_polyset_triangle_derivs(n, nderiv, pts);
  case cell::type::tetrahedron:
    return tabulate_polyset_tetrahedron_derivs(n, nderiv, pts);
  case cell::type::quadrilateral:
    return tabulate_polyset_quad_derivs(n, nderiv, pts);
  case cell::type::prism:
    return tabulate_polyset_prism_derivs(n, nderiv, pts);
  case cell::type::pyramid:
    return tabulate_polyset_pyramid_derivs(n, nderiv, pts);
  case cell::type::hexahedron:
    return tabulate_polyset_hex_derivs(n, nderiv, pts);
  default:
    throw std::runtime_error("Polynomial set: Unsupported cell type");
  }
}
//-----------------------------------------------------------------------------
int polyset::dim(cell::type celltype, int n)
{
  switch (celltype)
  {
  case cell::type::triangle:
    return (n + 1) * (n + 2) / 2;
  case cell::type::tetrahedron:
    return (n + 1) * (n + 2) * (n + 3) / 6;
  case cell::type::prism:
    return (n + 1) * (n + 1) * (n + 2) / 2;
  case cell::type::pyramid:
    return (n + 1) * (n + 2) * (2 * n + 3) / 6;
  case cell::type::interval:
    return (n + 1);
  case cell::type::quadrilateral:
    return (n + 1) * (n + 1);
  case cell::type::hexahedron:
    return (n + 1) * (n + 1) * (n + 1);
  default:
    return 1;
  }
}
//-----------------------------------------------------------------------------