////////////////////////////////////////////////////////////////////////////////
// CollapsePreventionEnergy.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  Sheet material energy density that prevents elements from collapsing into
//  degenerate configurations (which will break the bending energy...) with
//  an infinite energy barrier:
//      (-log((det(C) - activationThreshold) / activationThreshold + 1))^2
//  for det(C) < activationThreshold, 0 otherwise
//
//  This energy term is C1. We could make it C2 (to avoid the single point
//  where the Hessian is undefined) by raising the power from 2 to 3--at the
//  expense of a faster ramp-up (greater nonlinearity).
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Created:  05/30/2019 17:23:18
////////////////////////////////////////////////////////////////////////////////
#ifndef COLLAPSEPREVENTIONENERGY_HH
#define COLLAPSEPREVENTIONENERGY_HH
#include <cmath>

#include <Eigen/Dense>
#include <MeshFEM/EnergyDensities/Tensor.hh>
#include "SVDSensitivity.hh"
#include "InflatableSheet.hh"

struct BarrierFuncLogSq {
    using Real = InflatableSheet::Real;

    constexpr static Real inf = std::numeric_limits<double>::infinity();

    static Real   b(Real x) { if (x <= 0) return inf; if (x >= 1.0) return 0.0; return 0.5 * std::pow(log(x), 2); }
    static Real  db(Real x) { if (x <= 0) return inf; if (x >= 1.0) return 0.0; return log(x) / x; }
    static Real d2b(Real x) { if (x <= 0) return inf; if (x >= 1.0) return 0.0; return (1 - log(x)) / (x * x); }
};

template<class BarrierFunc>
struct NormalizedBarrierFunction {
    using Real = typename BarrierFunc::Real;
    using BF = BarrierFunc;

    void setActivationThreshold(Real val) { m_a = val; }
    Real activationThreshold() const { return m_a; }

    Real   b(Real x) const { return BF::  b(x / m_a); }
    Real  db(Real x) const { return BF:: db(x / m_a) / m_a; }
    Real d2b(Real x) const { return BF::d2b(x / m_a) / (m_a * m_a); }

protected:
    Real m_a = 1.0;
};

template<class BarrierFunc>
struct CollapsePreventionDet : public NormalizedBarrierFunction<BarrierFunc> {
    using BF = NormalizedBarrierFunction<BarrierFunc>;
    using M2d  = InflatableSheet::M2d;
    using Real = InflatableSheet::Real;

    template<typename Derived>
    void setMatrix(const Eigen::MatrixBase<Derived> &C) {
        static_assert((Derived::RowsAtCompileTime == 2) && (Derived::ColsAtCompileTime == 2), "Only 2x2 supported for now");
        m_det = C.determinant();
        m_grad_det <<  C(1, 1), -C(1, 0),
                      -C(0, 1),  C(0, 0);
    }

    Real energy() const { return BF::b(m_det); }
    M2d denergy() const { return BF::db(m_det) * m_grad_det; }

    template<typename Derived>
    M2d delta_denergy(const Eigen::MatrixBase<Derived> &dC) const {
        static_assert((Derived::RowsAtCompileTime == 2) && (Derived::ColsAtCompileTime == 2), "Only 2x2 supported for now");

        M2d delta_grad_det;
        delta_grad_det <<  dC(1, 1), -dC(1, 0),
                          -dC(0, 1),  dC(0, 0);

        return ((BF::d2b(m_det) * doubleContract(m_grad_det, dC.template cast<Real>()))) * m_grad_det
               + BF:: db(m_det) * delta_grad_det;
    }

    // For debugging scalar function of det + its derivatives
    void setDet(Real det) { m_det = det; }
    Real det() const { return m_det; }
    Real normalizedDet()  const { return m_det / BF::m_a; }
    Real denergy_ddet()   const { return BF::db(m_det);  }
    Real d2energy_d2det() const { return BF::d2b(m_det); }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
    Real m_det;
    M2d m_grad_det;
};

template<class BarrierFunc>
struct CollapsePreventionSingularValues : public NormalizedBarrierFunction<BarrierFunc> {
    using BFRaw = BarrierFunc;
    using BF    = NormalizedBarrierFunction<BarrierFunc>;
    using V2d   = InflatableSheet::V2d;
    using M2d   = InflatableSheet::M2d;
    using Real  = InflatableSheet::Real;

    // The activation threshold for the singular value barriers should be the
    // square root of the area barrier activation threshold.
    void setActivationThreshold(Real val) { BF::setActivationThreshold(std::sqrt(val)); }
    Real activationThreshold() const { return std::pow(BF::activationThreshold(), 2); }

    template<typename Derived>
    void setMatrix(const Eigen::MatrixBase<Derived> &F) { m_svd.setMatrix(F); m_det = F.determinant(); }

    Real energy() const {
        if (m_det < 0.0) return std::numeric_limits<double>::infinity();
        Real result = BF::b(m_svd.sigma(0)) + BF::b(m_svd.sigma(1));
        if (applyStretchBarrier) {
            Real scale = 1.0 / (stretchBarrierLimit - stretchBarrierActivation);
            result += BFRaw::b(scale * (stretchBarrierLimit - m_svd.sigma(0)))
                   +  BFRaw::b(scale * (stretchBarrierLimit - m_svd.sigma(1)));
        }
        return result;
    }

    M2d denergy() const {
        if (m_det < 0.0) {
            M2d result;
            result.setConstant(std::numeric_limits<double>::infinity());
        }
        V2d dE_dsigma(BF::db(m_svd.sigma(0)),
                      BF::db(m_svd.sigma(1)));
        if (applyStretchBarrier) {
            Real scale = 1.0 / (stretchBarrierLimit - stretchBarrierActivation);
            dE_dsigma -= scale * V2d(BFRaw::db(scale * (stretchBarrierLimit - m_svd.sigma(0))),
                                     BFRaw::db(scale * (stretchBarrierLimit - m_svd.sigma(1))));
        }

        return m_svd.U() * (dE_dsigma.asDiagonal() * m_svd.V().transpose());
    }

    const SVDSensitivity &svd() const { return m_svd; }
    Real det() const { return m_det; }

    // Second derivatives blow up when sigma_0 == sigma_1!!!
    template<typename Derived>
    M2d delta_denergy(const Eigen::MatrixBase<Derived> &/* dF */) const {
        static_assert((Derived::RowsAtCompileTime == 2) && (Derived::ColsAtCompileTime == 2), "Only 2x2 supported for now");
        throw std::runtime_error("Second derivative of SVD collapse prevention unsupported; will blow up at sigma_0 == sigma_2.");
    #if 0
        // SVDSensitivity doesn't yet implement delta_dsigma...
        return (BF::d2b(m_svd.sigma(0)) * m_svd.dsigma(0, dF)) * m_svd.dsigma(0) +
               (BF::d2b(m_svd.sigma(1)) * m_svd.dsigma(1, dF)) * m_svd.dsigma(1) +
                BF:: db(m_svd.sigma(0)) * m_svd.delta_dsigma(0, dF) +
                BF:: db(m_svd.sigma(1)) * m_svd.delta_dsigma(1, dF);
    #endif
    }

    bool applyStretchBarrier = false;
    Real stretchBarrierActivation = 1.75; // threshold below which barrier term is smoothly deactivated
    Real stretchBarrierLimit      = 2.25; // placement of the infinite barrier

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
    SVDSensitivity m_svd;
    Real m_det;
};

using CollapsePreventionEnergyDet = CollapsePreventionDet<BarrierFuncLogSq>;
using CollapsePreventionEnergySV  = CollapsePreventionSingularValues<BarrierFuncLogSq>;

#endif /* end of include guard: COLLAPSEPREVENTIONENERGY_HH */


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2015 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <iostream>

#include <boost/timer/timer.hpp>

#include "dune/grid/config.h"
#include "dune/grid/uggrid.hh"

#include "fem/assemble.hh"
#include "fem/norms.hh"
#include "fem/lagrangespace.hh"
//#include "fem/hierarchicspace.hh"   // ContinuousHierarchicMapper
#include "linalg/direct.hh"
#include "linalg/trivialpreconditioner.hh"
//#include "linalg/partialDirectPreconditioner.hh"
#include "linalg/additiveschwarz.hh"
#include "linalg/iluprecond.hh"      // PrecondType::ILUT, PrecondType::ILUK, PrecondType::ARMS
#include "linalg/iccprecond.hh"
#include "linalg/icc0precond.hh"
#include "linalg/hyprecond.hh"       // BoomerAMG, Euclid
#include "linalg/jacobiPreconditioner.hh"
#include "linalg/cg.hh"
#include "mg/hb.hh"
#include "utilities/enums.hh"
#include "utilities/gridGeneration.hh" //  createUnitSquare, createUnitCube
#include "io/vtk.hh"
#include "io/gnuplot.hh"
//#include "io/amira.hh"
#include "utilities/kaskopt.hh"

using namespace Kaskade;
#include "ht.hh"

int main(int argc, char *argv[])
{
  using namespace boost::fusion;

  std::cout << "Start heat transfer tutorial program" << std::endl;

  boost::timer::cpu_timer totalTimer;

  int verbosityOpt = 1;
  bool dump = true;
  std::unique_ptr<boost::property_tree::ptree> pt = getKaskadeOptions(argc, argv, verbosityOpt, dump);

  int  refinements = getParameter(pt, "refinements", 5),
       order       =  getParameter(pt, "order", 2),
       verbosity   = getParameter(pt, "verbosity", 1);
  std::cout << "original mesh shall be refined : " << refinements << " times" << std::endl;
  std::cout << "discretization order           : " << order << std::endl;
  std::cout << "output level (verbosity)       : " << verbosity << std::endl;

  int  direct, onlyLowerTriangle = false;

  DirectType directType;
//  IterateType iterateType = IterateType::CG;
  MatrixProperties property = MatrixProperties::SYMMETRIC;
  PrecondType precondType = PrecondType::NONE;
  std::string empty;

  std::string s("names.type.");
  s += getParameter(pt, "solver.type", empty);
  direct = getParameter(pt, s, 0);

  s = "names.direct." + getParameter(pt, "solver.direct", empty);
  directType = static_cast<DirectType>(getParameter(pt, s, 0));

//  s = "names.iterate." + getParameter(pt, "solver.iterate", empty);
//  iterateType = static_cast<IterateType>(getParameter(pt, s, 0));
  s = "names.preconditioner." + getParameter(pt, "solver.preconditioner", empty);
  precondType = static_cast<PrecondType>(getParameter(pt, s, 0));

  property = MatrixProperties::SYMMETRIC;

  if ( (directType == DirectType::MUMPS)||(directType == DirectType::PARDISO) || ( (precondType == PrecondType::ICC) && !direct ) )
  {
    onlyLowerTriangle = true;
    std::cout <<
      "Note: direct solver MUMPS/PARADISO or PrecondType::ICC preconditioner ===> onlyLowerTriangle is set to true!"
      << std::endl;
  }

  boost::timer::cpu_timer gridTimer;
//   two-dimensional space: dim=2
  constexpr int dim=2;
  using Grid = Dune::UGGrid<dim>;
  using LeafView = Grid::LeafGridView;
  using H1Space = FEFunctionSpace<ContinuousLagrangeMapper<double,LeafView> >;
  // using H1Space = FEFunctionSpace<ContinuousHierarchicMapper<double,LeafView> >;
  using Spaces = boost::fusion::vector<H1Space const*>;
  using VariableDescriptions = boost::fusion::vector<Variable<SpaceIndex<0>,Components<1>,VariableId<0> > >;
  using VariableSet = VariableSetDescription<Spaces,VariableDescriptions>;
  using Functional = HeatFunctional<double,VariableSet>;
  using Assembler = VariationalFunctionalAssembler<LinearizationAt<Functional> >;
  constexpr int neq = Functional::TestVars::noOfVariables;
  using CoefficientVectors = VariableSet::CoefficientVectorRepresentation<0,neq>::type;
  using LinearSpace = VariableSet::CoefficientVectorRepresentation<0,neq>::type;

  GridManager<Grid> gridManager( createUnitSquare<Grid>() );
  gridManager.globalRefine(refinements);
  std::cout << std::endl << "Grid: " << gridManager.grid().size(0) << " triangles, " << std::endl;
  std::cout << "      " << gridManager.grid().size(1) << " edges, " << std::endl;
  std::cout << "      " << gridManager.grid().size(2) << " points" << std::endl;
  std::cout << "computing time for generation of initial mesh: " << (double)(gridTimer.elapsed().user)/1e9 << "s\n";

  // construction of finite element space for the scalar solution T.
  H1Space temperatureSpace(gridManager,gridManager.grid().leafGridView(),order);

  Spaces spaces(&temperatureSpace);

  // construct variable list.
  // VariableDescription<int spaceId, int components, int Id>
  // spaceId: number of associated FEFunctionSpace
  // components: number of components in this variable
  // Id: number of this variable

  std::string varNames[1] = { "u" };

  VariableSet variableSet(spaces,varNames);

  // construct variational functional

  double kappa = 1.0;
  double q = 1.0;
  Functional F(kappa,q);
  constexpr int nvars = Functional::AnsatzVars::noOfVariables;
  std::cout << std::endl << "no of variables = " << nvars << std::endl;
  std::cout << "no of equations = " << neq   << std::endl;
  size_t dofs = variableSet.degreesOfFreedom(0,nvars);
  std::cout << "number of degrees of freedom = " << dofs   << std::endl;

  //construct Galerkin representation

  Assembler assembler(gridManager,spaces);
  VariableSet::VariableSet u(variableSet);
  VariableSet::VariableSet du(variableSet);

  size_t nnz = assembler.nnz(0,neq,0,nvars,onlyLowerTriangle);
  std::cout << "number of nonzero elements in the stiffness matrix: " << nnz << std::endl << std::endl;
  boost::timer::cpu_timer assembTimer;

  CoefficientVectors solution(VariableSet::CoefficientVectorRepresentation<0,neq>::init(spaces));
  solution = 0;

  assembler.assemble(linearization(F,u));
  CoefficientVectors rhs(assembler.rhs());
  AssembledGalerkinOperator<Assembler,0,neq,0,nvars> A(assembler, onlyLowerTriangle);
  MatrixAsTriplet<double> tri = A.get<MatrixAsTriplet<double> >();
  std::cout << "computing time for assemble: " << (double)(assembTimer.elapsed().user)/1e9 << "s\n";

//     for (k=0; k< nnz; k++)
//       {
//         printf("%3d %3d %e\n", tri.ridx[k], tri.cidx[k], tri.data[k]);
//       }

  if (direct)
  {
    boost::timer::cpu_timer directTimer;
    directInverseOperator(A,directType,property).applyscaleadd(-1.0,rhs,solution);
    u.data = solution.data;
    std::cout << "computing time for direct solve: " << (double)(directTimer.elapsed().user)/1e9 << "s\n";
  }
  else
  {
    boost::timer::cpu_timer iteTimer;
    Dune::InverseOperatorResult res;
    const DefaultDualPairing<LinearSpace,LinearSpace> defaultScalarProduct{};
    int iteSteps = getParameter(pt, "solver.iteMax", 2000);
    double iteEps = getParameter(pt, "solver.iteEps", 1.0e-10);
    StrakosTichyPTerminationCriterion<double> termination(iteEps,iteSteps);
    int lookAhead;
    switch (precondType)
    {
      case PrecondType::NONE:
      case PrecondType::HB:   lookAhead=50; break;
      default:                lookAhead=3; break;
    }
    lookAhead = getParameter(pt, "solver.lookAhead", lookAhead);
    termination.setLookAhead(lookAhead);

    switch (precondType)
    {
      case PrecondType::NONE:
      {
        std::cout << "selected preconditioner: NONE" << std::endl;
        TrivialPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > trivial;
        CG<LinearSpace,LinearSpace> cg(A,trivial,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
      }
      break;
      case PrecondType::ADDITIVESCHWARZ:
      {
        std::cout << "selected preconditioner: ADDITIVESCHWARZ" << std::endl;
        std::pair<size_t,size_t> idx = temperatureSpace.mapper().globalIndexRange(gridManager.grid().leafIndexSet().geomTypes(dim)[0]);
        AdditiveSchwarzPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > addschwarz(A,idx.first,idx.second,verbosity);
        CG<LinearSpace,LinearSpace> cg(A,addschwarz,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
      }
      break;
      case PrecondType::ILUT:
      {
        std::cout << "selected preconditioner: ILUT" << std::endl;
//        std::cout << "Note that this preconditioner combined with the BICGSTAB solver" << std::endl;
        std::cout << "needs matrix.property = GENERAL" << std::endl;
        int lfil = getParameter(pt, "solver.ILUT.lfil", 140);
        double dropTol = getParameter(pt, "solver.ILUT.dropTol", 0.01);
        ILUTPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > ilut(A,lfil,dropTol,verbosity);
        CG<LinearSpace,LinearSpace> cg(A,ilut,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
//         Dune::BiCGSTABSolver<LinearSpace> cg(A,ilut,iteEps,iteSteps,verbosity);
//         cg.apply(solution,rhs,res);
      }
      break;
      case PrecondType::ILUK:
      {
        std::cout << "selected preconditioner: ILUK" << std::endl;
//        std::cout << "Note that this preconditioner combined with the BICGSTAB solver" << std::endl;
        std::cout << "needs matrix.property = GENERAL" << std::endl;
        int fill_lev = getParameter(pt, "solver.ILUK.fill_lev", 3);
        ILUKPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > iluk(A,fill_lev,verbosity);
        CG<LinearSpace,LinearSpace> cg(A,iluk,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
//         Dune::BiCGSTABSolver<LinearSpace> cg(A,iluk,iteEps,iteSteps,verbosity);
//         cg.apply(solution,rhs,res);
      }
      break;
      case PrecondType::ARMS:
      {
        int lfil = getParameter(pt, "solver.ARMS.lfil", 140);
        int lev_reord = getParameter(pt, "solver.ARMS.lev_reord", 1);
        double dropTol = getParameter(pt, "solver.ARMS.dropTol", 0.01);
        double tolind = getParameter(pt, "solver.ARMS.tolind", 0.2);
        ARMSPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > iluk(A,lfil,dropTol,lev_reord,tolind,verbosity);
        CG<LinearSpace,LinearSpace> cg(A,iluk,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
      }
      break;
      case PrecondType::ICC:
      {
        std::cout << "selected preconditioner: ICC" << std::endl;
        if (property != MatrixProperties::SYMMETRIC)
        {
          std::cout << "PrecondType::ICC preconditioner of TAUCS lib has to be used with matrix.property==MatrixProperties::SYMMETRIC\n";
          std::cout << "i.e., call the executable with option --solver.property MatrixProperties::SYMMETRIC\n\n";
        }
        double dropTol = getParameter(pt, "solver.ICC.dropTol", 0.01);;
        ICCPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > icc(A,dropTol);
        CG<LinearSpace,LinearSpace> cg(A,icc,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
      }
      break;
      case PrecondType::ICC0:
      {
        std::cout << "selected preconditioner: ICC0" << std::endl;
        ICC_0Preconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > icc0(A);
        CG<LinearSpace,LinearSpace> cg(A,icc0,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
      }
      break;
      case PrecondType::HB:
      {
        std::cout << "selected preconditioner: HB" << std::endl;
        HierarchicalBasisPreconditioner<Grid,AssembledGalerkinOperator<Assembler,0,neq,0,nvars>::range_type, AssembledGalerkinOperator<Assembler,0,neq,0,nvars>::range_type > hb(gridManager.grid());
        CG<LinearSpace,LinearSpace> cg(A,hb,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
      }
      break;
      case PrecondType::BOOMERAMG:
      {
        int steps = getParameter(pt, "solver.BOOMERAMG.steps", iteSteps);
        int coarsentype = getParameter(pt, "solver.BOOMERAMG.coarsentype", 21);
        int interpoltype = getParameter(pt, "solver.BOOMERAMG.interpoltype", 0);
        int cycleType = getParameter(pt, "solver.BOOMERAMG.cycleType", 1);
        int relaxType = getParameter(pt, "solver.BOOMERAMG.relaxType", 3);
        int variant = getParameter(pt, "solver.BOOMERAMG.variant", 0);
        int overlap = getParameter(pt, "solver.BOOMERAMG.overlap", 1);
        double tol = getParameter(pt, "solver.BOOMERAMG.tol", iteEps);
        double strongThreshold = getParameter(pt, "solver.BOOMERAMG.strongThreshold", (dim==2)?0.25:0.6);
        BoomerAMG<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> >
                  boomerAMGPrecon(A,steps,coarsentype,interpoltype,tol,cycleType,relaxType,
                  strongThreshold,variant,overlap,1,verbosity);
        CG<LinearSpace,LinearSpace> cg(A,boomerAMGPrecon,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
//         Dune::LoopSolver<LinearSpace> cg(A,boomerAMGPrecon,iteEps,iteSteps,verbosity);
//         cg.apply(solution,rhs,res);
      }
      break;
      case PrecondType::EUCLID:
      {
        std::cout << "selected preconditioner: EUCLID" << std::endl;
        int level      = getParameter(pt, "solver.EUCLID.level",1);
        double droptol = getParameter(pt, "solver.EUCLID.droptol",0.01);
        int printlevel = 0;
        if (verbosity>2) printlevel=verbosity-2;
        printlevel = getParameter(pt,"solver.EUCLID.printlevel",printlevel);
        int bj = getParameter(pt, "solver.EUCLID.bj",0);
        Euclid<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > EuclidPrecon(A,level,droptol,printlevel,bj,verbosity);
        CG<LinearSpace,LinearSpace> cg(A,EuclidPrecon,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
     }
      break;
      case PrecondType::JACOBI:
      default:
      {
        std::cout << "selected preconditioner: JACOBI" << std::endl;
        JacobiPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > jacobi(A,1.0);
        CG<LinearSpace,LinearSpace> cg(A,jacobi,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
      }
      break;
    }
    solution *= -1.0;
    u.data = solution.data;

    std::cout << "iterative solve eps= " << iteEps << ": "
              << (res.converged?"converged":"failed") << " after "
              << res.iterations << " steps, rate="
              << res.conv_rate << ", computing time=" << (double)(iteTimer.elapsed().user)/1e9 << "s\n";
  }

  // compute L2 norm of the solution
  boost::timer::cpu_timer outputTimer;
  L2Norm l2Norm;
  std::cout << "L2norm(solution) = " << l2Norm(boost::fusion::at_c<0>(u.data)) << std::endl;

  // output of solution in VTK format for visualization,
  // the data are written as ascii stream into file temperature.vtu,
  // possible is also binary
  writeVTKFile(u,"temperature",IoOptions().setOrder(order));
  std::cout << "graphical output finished, data in VTK format is written into file temperature.vtu \n";
  IoOptions gnuplotOptions{};
  //    gnuplotOptions.info = IoOptions::none; // or IoOptions::summary or IoOptions::detail
  writeGnuplotFile(u,"temperature",gnuplotOptions);
  std::cout << "graphical output finished, Gnuplot data are written into file temperature.data \n";

  // output of solution for Amira visualization,
  // the data are written in binary format into file temperature.am,
  // possible is also ascii
  // IoOptions options;
  // options.outputType = IoOptions::ascii;
  // LeafView leafGridView = gridManager.grid().leafGridView();
  // writeAMIRAFile(leafGridView,variableSet,u,"temperature",options);

  std::cout << "computing time for output: " << (double)(outputTimer.elapsed().user)/1e9 << "s\n";

  std::cout << "total computing time: " << (double)(totalTimer.elapsed().user)/1e9 << "s\n";
  std::cout << "End heat transfer tutorial program" << std::endl;
}


#include <stdio.h>
#include <iostream>
#include <g2o/core/block_solver.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/types/slam2d/types_slam2d.h>
#include <Eigen/Core>

typedef struct{
    int s, e;
    Eigen::Vector2d pose;
} Edge;
typedef g2o::BlockSolver< g2o::BlockSolverTraits<2, 2> >  SlamBlockSolver;
typedef g2o::LinearSolverEigen<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;

int main(int argc, const char * argv[]) {

    std::vector<Edge> edgeData = {
        {0, 1, {1, 1}},
        {1, 2, {1, -1}},
        {2, 3, {-1, -1}},
        {3, 0, {-0.5, 0.5}},
    };

    std::unique_ptr<SlamLinearSolver> linearSolver = g2o::make_unique<SlamLinearSolver>();
    linearSolver->setBlockOrdering(false);
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<SlamBlockSolver>(std::move(linearSolver)));

    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    auto maxEdge = std::max_element(edgeData.begin(), edgeData.end(), [](const Edge& a, const Edge& b){
        return std::max(a.e, a.s) < std::max(b.e, b.s);
    });
    int maxIndex = std::max(maxEdge->s, maxEdge->e);

    for(int i = 0; i < maxIndex+1; i++){
        g2o::VertexPointXY *v = new g2o::VertexPointXY();
        v->setId(i);
        v->setEstimate(g2o::Vector2());
        if(i == 0){
            v->setFixed(true);
        }
        optimizer.addVertex(v);
    }

    for(const auto& pData: edgeData){
        g2o::EdgePointXY* edge = new g2o::EdgePointXY();
        edge->setVertex( 0, optimizer.vertex(pData.s));
        edge->setVertex( 1, optimizer.vertex(pData.e));
        edge->setInformation(  Eigen::Matrix< double, 2,2 >::Identity() );// 信息矩阵表示2维上侧重哪一维，xy是一样重要的，所以就是单位矩阵，但是在6维的位姿中，有可能更侧重优化旋转或者位移，就需要设置信息矩阵
        edge->setMeasurement(pData.pose );
        optimizer.addEdge(edge);
    }

    optimizer.initializeOptimization();
    optimizer.optimize(500);
    for(int i = 0; i < maxIndex+1; i++){
        g2o::VertexPointXY* vertex = dynamic_cast<g2o::VertexPointXY*>(optimizer.vertex( i ));
        g2o::Vector2 pose = vertex->estimate();
        std::cout << i << ":\n" << pose << std::endl ;
    }
    return 0;
}


#include <string>
#include <fstream>
#include <vector>
#include <utility> // std::pair
#include <sstream>
#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

using namespace std;

vector<vector<double>> read_csv( string filename ){
    // Reads a CSV file with 2 columns into a vector of <vector<double>>
    vector<vector<double>> result(2);

    // Create an input filestream
	//const char *c = filename.c_str();
    ifstream myFile(filename.c_str());

    // Make sure the file is open
    if(!myFile.is_open()) throw runtime_error("Could not open file");

    // Helper vars
    string line;
    double val;

	if(myFile.good())
	{
		// Read data, line by line
		while(getline(myFile, line))
		{
			// Create a stringstream of the current line
			stringstream ss(line);

			// Keep track of the current column index
			int colIdx = 0;
			// Extract each integer
			while(ss >> val){

				// Add the current integer to the 'colIdx' column's values vector
				result[colIdx].push_back(val);

				// If the next token is a comma, ignore it and move on
				if(ss.peek() == ',') ss.ignore();

				// Increment the column index
				colIdx++;
			}
		}
	}

    // Close file
    myFile.close();
    return result;
}

double getyL(string ref, double positionx , double positiony, bool debug)
{
	typedef boost::geometry::model::d2::point_xy<double> point_type;
	typedef boost::geometry::model::linestring<point_type> linestring_type;

	if (debug) cout << "Entry: Read CSV" << endl;
    vector<vector<double>> gold_ref = read_csv(ref);
	if (debug) cout << "Exit: Read CSV" << endl;

	point_type p(positionx, positiony);
	linestring_type line;
	double x = 0;
	double y = 0;

	for (int i = 0; i < gold_ref[0].size(); ++i)
    {
		for(int j = 0; j < gold_ref.size(); ++j)
		{
			if (j == 0) x = gold_ref[j][i];
			else y = gold_ref[j][i];

		}
		line.push_back(point_type(x,y));
        //cout << x << " , " << y << "\n" ;
    }

	double yl = boost::geometry::distance(p, line);
	//cout << "Point-Line: " << yl << endl;
	return yl;
}

/*int main(int argc , char *argv[])
  {
	typedef boost::geometry::model::d2::point_xy<double> point_type;
	typedef boost::geometry::model::linestring<point_type> linestring_type;
    vector<vector<double>> gold_ref = read_csv("/home/sayandipde/Approx_IBC/hil/client/Webots/worlds/city_ref.csv");

	point_type p(1,2);
	linestring_type line;
	double x = 0;
	double y = 0;

	for (int i = 0; i < gold_ref[0].size(); ++i)
    {
		for(int j = 0; j < gold_ref.size(); ++j)
		{
			//cout << gold_ref[j][i];
			//if(j != gold_ref.size() - 1) cout << ","; // No comma at end of line
			if (j == 0) x = gold_ref[j][i];
			else y = gold_ref[j][i];

		}
		line.push_back(point_type(x,y));
        cout << x << " , " << y << "\n" ;
    }

	cout << "Point-Line: " << boost::geometry::distance(p, line) << endl;
	return 0;
  }*/


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file decorrelated_gaussian.hpp
 * \date JUly 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <vector>
#include <string>
#include <cstddef>
#include <type_traits>

#include <fl/util/traits.hpp>
#include <fl/exception/exception.hpp>
#include <fl/distribution/interface/moments.hpp>
#include <fl/distribution/interface/evaluation.hpp>
#include <fl/distribution/interface/standard_gaussian_mapping.hpp>
#include <fl/distribution/gaussian.hpp>

namespace fl
{

/**
 * \class DecorrelatedGaussian
 *
 * \brief General Decorrelated Gaussian Distribution
 * \ingroup distributions
 * \{
 *
 * The Gaussian is a general purpose distribution representing a multi-variate
 * \f${\cal N}(x; \mu, \Sigma)\f$. It can be used in various
 * ways while maintaining efficienty at the same time. This is due to it's
 * multi-representation structure. The distribution can be represented either by
 *
 *  - the covariance matrix \f$\Sigma\f$,
 *  - the precision matrix \f$\Sigma^{-1} = \Lambda\f$,
 *  - the covariance square root matrix (Cholesky decomposition or LDLT)
 *    \f$\sqrt{\Sigma} = L\sqrt{D}\f$,
 *  - or the diagonal form of the previous three options
 *    \f$diag(\sigma_1, \ldots, \sigma_n)\f$.
 *
 * A change in one representation results in change of all other
 * representations.
 *
 * Two key features of the distribution are its aibility to evaluation the
 * probability of a given sample and to map a noise sample into the distribution
 * sample space.
 *
 * \cond internal
 * The Gaussian internal structure uses lazy assignments or write on read
 * technique. Due to the multi-representation of the Gaussian, modifying one
 * representation affects all remaining ones. If one of the representation is
 * modified, the other representations are only then updated when needed. This
 * minimizes redundant computation and increases efficienty.
 * \endcond
 */
template <typename Variate>
class DecorrelatedGaussian
    : public Moments<Variate, typename DiagonalSecondMomentOf<Variate>::Type>,
      public Evaluation<Variate>,
      public StandardGaussianMapping<Variate, SizeOf<Variate>::Value>
{
public:
    typedef Evaluation<Variate> EvaluationInterface;

    typedef Moments<
                Variate, typename DiagonalSecondMomentOf<Variate>::Type
            > MomentsInterface;

    typedef StandardGaussianMapping<
                Variate, SizeOf<Variate>::Value
            > StdGaussianMappingInterface;

    /**
     * \brief Second moment matrix type, i.e covariance matrix, precision
     *        matrix, and their diagonal and square root representations
     */
    typedef typename DiagonalSecondMomentOf<Variate>::Type DiagonalSecondMoment;

    typedef typename SecondMomentOf<Variate>::Type DenseSecondMoment;

    /**
     * \brief Represents the StandardGaussianMapping standard variate type which
     *        is of the same dimension as the Gaussian Variate. The
     *        StandardVariate type is used to sample from a standard normal
     *        Gaussian and map it to this Gaussian
     */
    typedef
    typename StdGaussianMappingInterface::StandardVariate StandardVariate;

protected:
    /** \cond internal */
    /**
     * \enum Attribute
     * Implementation attributes. The enumeration lists the different
     * representations along with other properties such as the rank of the
     * second moment and the log normalizer.
     */
    enum Attribute
    {
        DiagonalCovarianceMatrix = 0,/**< Diagonal form of of cov. mat. */
        DiagonalPrecisionMatrix,     /**< Diagonal form of inv cov. mat. */
        DiagonalSquareRootMatrix,    /**< Diagonal form of Cholesky decomp. */
        Rank,                        /**< Covariance Rank */
        Normalizer,                  /**< Log probability normalizer */
        Determinant,                 /**< Determinant of covariance */

        Attributes                   /**< Total number of attribute */
    };

    /**
     * \brief Flags array type which contains the content status if different
     *        distribution representation
     */
    typedef std::array<bool, Attributes> FlagArray;
    /** \endcond */

public:
    /**
     * Creates a dynamic or fixed size Gaussian.
     *
     * \param dimension Dimension of the Gaussian. The default is defined by the
     *                  dimension of the variable type \em Vector. If the size
     *                  of the Vector at compile time is fixed, this will be
     *                  adapted. For dynamic-sized Variable the dimension is
     *                  initialized to 0.
     */
    explicit DecorrelatedGaussian(int dim = DimensionOf<Variate>()):
        StdGaussianMappingInterface(dim)
    {
        static_assert(SizeOf<Variate>::Value != 0, "Illegal static dimension");

        std::fill(dirty_.begin(), dirty_.end(), true);
        set_standard();
    }

    /**
     * \brief Overridable default destructor
     */
    virtual ~DecorrelatedGaussian() noexcept { }

    /**
     * \return Gaussian dimension
     */
    virtual int dimension() const
    {
        return StdGaussianMappingInterface::standard_variate_dimension();
    }

    /**
     * \return Gaussian first moment
     */
    virtual const Variate& mean() const
    {
        return mean_;
    }

    /**
     * \return Gaussian second centered moment
     *
     * Computes the covariance from other representation of not available
     *
     * \throws GaussianUninitializedException if the Gaussian is of dynamic-size
     *         and has not been initialized using SetStandard(dimension).
     * \throws InvalidGaussianRepresentationException if non-of the
     *         representation can be used as a source
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#CovarianceMatrix}
     * \endcond
     */
    virtual const DiagonalSecondMoment& covariance() const
    {
        if (dimension() == 0)
        {
            fl_throw(GaussianUninitializedException());
        }

        if (is_dirty(DiagonalCovarianceMatrix))
        {
            switch (select_first_representation<2>(
                        {{ DiagonalSquareRootMatrix,
                           DiagonalPrecisionMatrix }}))
            {
            case DiagonalSquareRootMatrix:
            {
                covariance_.diagonal() = square_root_.diagonal().cwiseProduct(
                                            square_root_.diagonal());
             } break;

            case DiagonalPrecisionMatrix:
            {
                covariance_.diagonal() = precision_.diagonal().cwiseInverse();
            } break;

            default:
                fl_throw(InvalidGaussianRepresentationException());
                break;
            }

            updated_internally(DiagonalCovarianceMatrix);
        }

        return covariance_;
    }

    /**
     * \return Gaussian second centered moment in the precision form (inverse
     * of the covariance)
     *
     * Computes the precision from other representation of not available
     *
     * \throws GaussianUninitializedException if the Gaussian is of dynamic-size
     *         and has not been initialized using SetStandard(dimension).
     * \throws InvalidGaussianRepresentationException if non-of the
     *         representation can be used as a source
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#PrecisionMatrix}
     * \endcond
     */
    virtual const DiagonalSecondMoment& precision() const
    {
        if (dimension() == 0)
        {
            fl_throw(GaussianUninitializedException());
        }

        if (is_dirty(DiagonalPrecisionMatrix))
        {
            switch (select_first_representation<2>(
                        {{ DiagonalCovarianceMatrix,
                           DiagonalSquareRootMatrix}}))
            {
            case DiagonalCovarianceMatrix:
            case DiagonalSquareRootMatrix:
                precision_.diagonal() = covariance().diagonal().cwiseInverse();
                break;

            default:
                fl_throw(InvalidGaussianRepresentationException());
                break;
            }

            updated_internally(DiagonalPrecisionMatrix);
        }

        return precision_;
    }

    /**
     * \return Gaussian second centered moment in the square root form (
     * Cholesky decomposition)
     *
     * Computes the square root from other representation of not available
     *
     * \throws GaussianUninitializedException if the Gaussian is of dynamic-size
     *         and has not been initialized using SetStandard(dimension).
     * \throws InvalidGaussianRepresentationException if non-of the
     *         representation can be used as a source
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#SquareRootMatrix}
     * \endcond
     */
    virtual const DiagonalSecondMoment& square_root() const
    {
        if (dimension() == 0)
        {
            fl_throw(GaussianUninitializedException());
        }

        if (is_dirty(DiagonalSquareRootMatrix))
        {
            switch (select_first_representation<2>(
                        {{ DiagonalCovarianceMatrix,
                           DiagonalPrecisionMatrix }}))
            {
            case DiagonalCovarianceMatrix:
            case DiagonalPrecisionMatrix:
            {
                square_root_.diagonal() = covariance().diagonal().cwiseSqrt();
            } break;

            default:
                fl_throw(InvalidGaussianRepresentationException());
                break;
            }

            updated_internally(DiagonalSquareRootMatrix);
        }

        return square_root_;
    }

    /**
     * \return True if the covariance matrix has a full rank
     *
     * \throws see covariance()
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#CovarianceMatrix}
     * \endcond
     */
    virtual bool has_full_rank() const
    {
        if (is_dirty(Rank))
        {
            full_rank_ = true;

            switch (select_first_representation<3>(
                        {{ DiagonalCovarianceMatrix,
                           DiagonalPrecisionMatrix,
                           DiagonalSquareRootMatrix }}))
            {
            case DiagonalCovarianceMatrix:
                full_rank_ = has_full_rank(covariance());
                break;
            case DiagonalPrecisionMatrix:
                full_rank_ = has_full_rank(precision());
                break;
            case DiagonalSquareRootMatrix:
                full_rank_ = has_full_rank(square_root());
                break;
            default:
                fl_throw(InvalidGaussianRepresentationException());
                break;
            }

            updated_internally(Rank);
        }

        return full_rank_;
    }

    /**
     * \return Log normalizing constant
     *
     * \throws see has_full_rank()
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#CovarianceMatrix}
     * \endcond
     */
    virtual Real log_normalizer() const
    {
        if (is_dirty(Normalizer))
        {
            if (has_full_rank())
            {
                log_norm_ = -0.5
                    * (log(covariance_determinant())
                       + Real(covariance().rows()) * log(2.0 * M_PI));
            }
            else
            {
                log_norm_ = 0.0; // FIXME
            }

            updated_internally(Normalizer);
        }

        return log_norm_;
    }

    /**
     * \return Covariance determinant
     *
     * \throws see covariance
     */
    virtual Real covariance_determinant() const
    {
        if (is_dirty(Determinant))
        {
            determinant_ = covariance().diagonal().prod();

            updated_internally(Determinant);
        }

        return determinant_;
    }

    /**
     * \return Log of the probability of the given sample \c vector
     *
     * \param vector sample which should be evaluated
     *
     * \throws see has_full_rank()
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#PrecisionMatrix}
     * \endcond
     */
    virtual Real log_probability(const Variate& vector) const
    {
        if(has_full_rank())
        {
            return log_normalizer() - 0.5
                    * (vector - mean()).transpose()
                    * precision()
                    * (vector - mean());
        }

        return -std::numeric_limits<Real>::infinity();
    }

    /**
     * \return a Gaussian sample of the type \c Vector determined by mapping a
     * noise sample into the Gaussian sample space
     *
     * \param sample    Noise Sample
     *
     * \throws see square_root()
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#SquareRootMatrix}
     * \endcond
     */
    virtual Variate map_standard_normal(const StandardVariate& sample) const
    {
        return mean() + square_root() * sample;
    }

    /**
     * Sets the Gaussian to a standard distribution with zero mean and identity
     * covariance.
     *
     * \cond internal
     * \pre {}
     * \post
     *  - Fully ranked covariance
     *  - {Valid representations} = {#CovarianceMatrix}
     * \endcond
     */
    virtual void set_standard()
    {
        mean_.resize(dimension());
        covariance_.resize(dimension());
        precision_.resize(dimension());
        square_root_.resize(dimension());

        mean(Variate::Zero(dimension()));

        auto cov = DiagonalSecondMoment(dimension());
        cov.setIdentity(dimension());
        covariance(cov);

        full_rank_ = true;
        updated_internally(Rank);
    }

    /**
     * Changes the dimension of the dynamic-size Gaussian and sets it to a
     * standard distribution with zero mean and identity covariance.
     *
     * \param new_dimension New dimension of the Gaussian
     *
     * \cond internal
     * \pre {}
     * \post
     *  - Fully ranked covariance
     *  - {Valid representations} = {#CovarianceMatrix}
     * \endcond
     *
     * \throws ResizingFixedSizeEntityException
     *         see GaussianMap::standard_variate_dimension(int)
     */
    virtual void dimension(int new_dimension)
    {
        StdGaussianMappingInterface::standard_variate_dimension(new_dimension);
        set_standard();
    }

    /**
     * Sets the mean
     *
     * \param mean New Gaussian mean
     *
     * \throws WrongSizeException
     */
    virtual void mean(const Variate& mean) noexcept
    {
        if (mean_.size() != mean.size())
        {
            fl_throw(fl::WrongSizeException(mean.size(), mean_.size()));
        }

        mean_ = mean;
    }

    /**
     * Sets the covariance matrix as a diagonal matrix
     *
     * \param diag_covariance New diagonal covariance matrix
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#DiagonalCovarianceMatrix}
     * \endcond
     *
     * \throws WrongSizeException
     */
    virtual void covariance(
        const DiagonalSecondMoment& diag_covariance) noexcept
    {
        if (diag_covariance.size() != covariance_.size())
        {
            fl_throw(
                fl::WrongSizeException(
                    diag_covariance.size(), covariance_.size()));
        }

        covariance_ = diag_covariance;
        updated_externally(DiagonalCovarianceMatrix);
    }

    /**
     * Sets the covariance matrix in its diagonal square root form
     *
     * \param diag_square_root New diagonal square root of the covariance
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#DiagonalSquareRootMatrix}
     * \endcond
     *
     * \throws WrongSizeException
     */
    virtual void square_root(
        const DiagonalSecondMoment& diag_square_root) noexcept
    {
        if (diag_square_root.size() != square_root_.size())
        {
            fl_throw(
                fl::WrongSizeException(
                    diag_square_root.size(), square_root_.size()));
        }

        square_root_ = diag_square_root;
        updated_externally(DiagonalSquareRootMatrix);
    }

    /**
     * Sets the covariance matrix in its diagonal precision form
     *
     * \param diag_precision New diagonal precision matrix
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#DiagonalPrecisionMatrix}
     * \endcond
     *
     * \throws WrongSizeException
     */
    virtual void precision(
        const DiagonalSecondMoment& diag_precision) noexcept
    {
        if (diag_precision.size() != precision_.size())
        {
            fl_throw(
                fl::WrongSizeException(
                    diag_precision.size(), precision_.size()));
        }

        precision_ = diag_precision;
        updated_externally(DiagonalPrecisionMatrix);
    }

    /**
     * Sets the covariance matrix as a diagonal matrix
     *
     * \param diag_covariance New diagonal covariance matrix
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#DiagonalCovarianceMatrix}
     * \endcond
     *
     * \throws WrongSizeException
     */
    virtual void covariance(
        const Eigen::MatrixBase<DenseSecondMoment>& cov) noexcept
    {
        covariance(cov.diagonal().asDiagonal());
    }

    /**
     * Sets the covariance matrix in its diagonal square root form
     *
     * \param diag_square_root New diagonal square root of the covariance
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#DiagonalSquareRootMatrix}
     * \endcond
     *
     * \throws WrongSizeException
     */
    virtual void square_root(
        const Eigen::MatrixBase<DenseSecondMoment>& sqrt) noexcept
    {
        square_root(sqrt.diagonal().asDiagonal());
    }

    /**
     * Sets the covariance matrix in its diagonal precision form
     *
     * \param diag_precision New diagonal precision matrix
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#DiagonalPrecisionMatrix}
     * \endcond
     *
     * \throws WrongSizeException
     */
    virtual void precision(
        const Eigen::MatrixBase<DenseSecondMoment>& prec) noexcept
    {
        precision(prec.diagonal().asDiagonal());
    }

protected:
    /** \cond internal */
    /**
     * Flags the specified attribute as valid and the rest of attributes as
     * dirty.
     *
     * \param attribute Modified attribute
     */
    virtual void updated_externally(Attribute attribute) const noexcept
    {
        std::fill(dirty_.begin(), dirty_.end(), true);
        updated_internally(attribute);
    }

    /**
     * Flags the specified attribute as valid.
     *
     * \param attribute Modified attribute
     */
    virtual void updated_internally(Attribute attribute) const noexcept
    {
        dirty_[attribute] = false;
    }

    /**
     * \return True if any of the other representation was modified.
     * \param attribute     Attribute in question
     */
    virtual bool is_dirty(Attribute attribute) const noexcept
    {
        return dirty_[int(attribute)];
    }

    /**
     * \return First representation ID that is available
     *
     * \param representations   Representation list
     *
     * Example:
     * If the last invoked functions were
     *
     * \code
     * diagonal_covariance(my_diagonal);
     * my_covariance = covariance();
     * \endcode
     *
     * Now, the representation is set to \c DiagonalCovarianceMatrix and
     * \c CovarianceMatrix since \c diagonal_covariance() was used to set the
     * covariance matrix followed by requesting \c covariance().
     * The following subsequent call
     *
     * \code
     * Attribute att = SelectRepresentation({SquareRoot,
     *                                       DiagonalCovarianceMatrix,
     *                                       CovarianceMatrix});
     * \endcode
     *
     * will assign att to DiagonalCovarianceMatrix since that is the first
     * available representation within the initializer-list
     * <tt>{#SquareRoot, #DiagonalCovarianceMatrix, #CovarianceMatrix}</tt>.
     *
     * This method is used to determine the best suitable representation
     * for conversion. It is recommanded to put the diagonal forms at the
     * beginning of the initialization-list. Diagonal forms can be converted
     * most efficiently other  representations.
     */
    template <int AttributeCount>
    Attribute select_first_representation(
        const std::array<Attribute, AttributeCount>& representations
    ) const noexcept
    {
        for (auto& rep: representations)  if (!is_dirty(rep)) return rep;
        return Attributes;
    }

    /**
     * \brief has_full_rank check implementation
     */
    virtual bool has_full_rank(const DiagonalSecondMoment& mat) const
    {
        bool full_rank = true;

        const auto& diag = mat.diagonal();

        for (int i = 0; i < diag.size(); ++i)
        {
            if (std::fabs(diag(i)) < 1e-24)
            {
                full_rank = false;
                break;
            }
        }

        return full_rank;
    }
    /** \endcond */

protected:
    /** \cond internal */
    Variate mean_;                            /**< \brief first moment vector */
    mutable DiagonalSecondMoment covariance_; /**< \brief cov. form */
    mutable DiagonalSecondMoment precision_;  /**< \brief cov. inverse form */
    mutable DiagonalSecondMoment square_root_;/**< \brief cov. square root  */
    mutable bool full_rank_;                  /**< \brief full rank flag */
    mutable Real log_norm_;                   /**< \brief log normalizing const */
    mutable Real determinant_;         /**< \brief determinant of covariance */
    mutable FlagArray dirty_;          /**< \brief data validity flags */
    /** \endcond */
};

/** \} */

}


/*
 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)
               );
    }

}}


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


/* ---------------------------------------------------------------------
 *
 * 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;
}