/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                       */
/*  This file is part of the library KASKADE 7                 */
/*    see http://www.zib.de/en/numerik/software/kaskade-7.html         */
/*                                       */
/*  Copyright (C) 2002-2011 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/embedded_errorest.hh"
#include "fem/lagrangespace.hh"
//#include "fem/hierarchicspace.hh"   // ContinuousHierarchicMapper
#include "linalg/direct.hh"
#include "linalg/trivialpreconditioner.hh"
#include "linalg/iluprecond.hh"      // PrecondType::ILUT, PrecondType::ILUK, PrecondType::ARMS
#include "linalg/iccprecond.hh"
#include "linalg/icc0precond.hh"
#include "linalg/hyprecond.hh"       // BoomerAMG
#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/amira.hh"
#include "utilities/kaskopt.hh"

//#include "cubus.hh"
using namespace Kaskade;
#include "peaksource.hh"

#ifndef SPACEDIM
#define SPACEDIM 2
#endif

#if SPACEDIM==2
#define DEFAULT_REFINEMENTS 5
#else
#define DEFAULT_REFINEMENTS 2
#endif

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

  std::cout << "Start heat transfer tutorial program using embedded error estimation" << 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, "refinement", DEFAULT_REFINEMENTS),
       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;

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

  int direct, onlyLowerTriangle = false;

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

  // the user may select a value for solver.direct of
  // the enumeration class {UMFPACK, PARDISO, MUMPS, SUPERLU, UMFPACK3264, UMFPACK64}
  // Remark: DirectType::PARDISO not yet available
  s = "names.direct." + getParameter(pt, "solver.direct", empty);
  directType = static_cast<DirectType>(getParameter(pt, s, 2));

  // the user may select a value for solver.iterate of
  // the enumeration {CG, BICGSTAB, GMRES, PCG, APCG, SGS}
  // Remark: in this example only IterateType::CG is used.
//  s = "names.iterate." + getParameter(pt, "solver.iterate", empty);
//  iterateType = static_cast<IterateType>(getParameter(pt, s, 0));

  // the user may select a value for solver.preconditioner of
  // the enumeration class {NONE, JACOBI, ILUT, ILUK, ARMS, ADDITIVESCHWARZ,
  //                  BOOMERAMG, EUCLID, SSOR, ICC0, ICC, ILUKS}
  // Remark: in this example only PrecondType::NONE,PrecondType::JACOBI, PrecondType::ICC, PrecondType::ICC0, PrecondType::BOOMERAMG are used.
  s = "names.preconditioner." + getParameter(pt, "solver.preconditioner", empty);
  precondType = static_cast<PrecondType>(getParameter(pt, s, 0));

  property = MatrixProperties::SYMMETRIC;
  std::cout << "discretization is symmetric" << std::endl;

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

#if SPACEDIM==2
  //   two-dimensional space: dim=2
  constexpr int dim=2;
  using Grid = Dune::UGGrid<dim>;
  GridManager<Grid> gridManager( createUnitSquare<Grid>() );
  gridManager.globalRefine(refinements);
  std::cout << std::endl << "Grid: " << gridManager.grid().size(0) << " triangles, " << std::endl;
#else
  //  three-dimensional space: dim=3
  constexpr int dim=3;
  using Grid = Dune::UGGrid<dim>;
  GridManager<Grid> gridManager( createUnitCube<Grid>(0.5) );
  gridManager.globalRefine(refinements);
  std::cout << std::endl << "Grid: " << gridManager.grid().size(0) << " tetrahedra, " << std::endl;
  std::cout << "      " << gridManager.grid().size(1) << " triangles, " << std::endl;
#endif
  std::cout << "      " << gridManager.grid().size(dim-1) << " edges, " << std::endl;
  std::cout << "      " << gridManager.grid().size(dim) << " points" << std::endl;

  using LeafView = Grid::LeafGridView;
  using H1Space = FEFunctionSpace<ContinuousLagrangeMapper<double,LeafView> >;
  using Spaces = boost::fusion::vector<H1Space const*>;
  // alternative: using H1Space = FEFunctionSpace<ContinuousHierarchicMapper<double,LeafView> >;
  using VariableDescriptions = boost::fusion::vector<VariableDescription<0,1,0> >;
  using VariableSet = VariableSetDescription<Spaces,VariableDescriptions>;
  using Functional = PeaksourceFunctional<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.setVerbosity(verbosity);
  gridManager.enforceConcurrentReads(true);

  // construction of finite element space for the scalar solution T
  H1Space temperatureSpace(gridManager,gridManager.grid().leafGridView(),
               order);
  Spaces spaces(&temperatureSpace);
  // 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] = { "T" };
  VariableSet variableSet(spaces,varNames);

  Functional F;

    //construct Galerkin representation

  constexpr int nvars = Functional::AnsatzVars::noOfVariables;
  std::cout << "no of variables = " << nvars << std::endl;
  std::cout << "no of equations = " << neq   << std::endl;

  Assembler assembler(gridManager,spaces);
  VariableSet::VariableSet xx(variableSet);

  size_t nnz  = assembler.nnz(0,neq,0,nvars,onlyLowerTriangle);
  size_t size = variableSet.degreesOfFreedom(0,nvars);
  if ( verbosity>0) std::cout << "init mesh: nnz = " << nnz << ", dof = " << size << std::endl;

  std::vector<std::pair<double,double> > tol(1);
  double atol = getParameter(pt, "solver.atol", 1.0e-5);
  double rtol = getParameter(pt, "solver.rtol", 1.0e-5);
  tol[0] = std::make_pair(atol,rtol);
  std::cout << std::endl << "Accuracy: atol = " << atol << ",  rtol = " << rtol << std::endl;

  bool accurate = true;
  int refSteps = -1;
  int iter=0;

  do {
    refSteps++;

    boost::timer::cpu_timer assembTimer;
    VariableSet::VariableSet x(variableSet);
    assembler.assemble(linearization(F,x));
    CoefficientVectors solution(VariableSet::CoefficientVectorRepresentation<0,neq>::init(spaces));
    solution = 0;
    CoefficientVectors rhs(assembler.rhs());
    AssembledGalerkinOperator<Assembler,0,1,0,1> A(assembler, onlyLowerTriangle);
    MatrixAsTriplet<double> tri = A.get<MatrixAsTriplet<double> >();
    if ( verbosity>1) std::cout << "assemble: " << (double)assembTimer.elapsed().user/1e9 << "s\n";

    if (direct) {
      boost::timer::cpu_timer directTimer;
      directInverseOperator(A,directType,property).applyscaleadd(-1.0,rhs,solution);
      x.data = solution.data;

      if ( verbosity>1) std::cout << "direct solve: " << (double)(directTimer.elapsed().user)/1e9 << "s\n";
    }
    else {
      //if ( verbosity>0) std::cout << "iterative solver: steps = " << iteSteps << ", eps = " << iteEps << std::endl;
      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:
        {
          TrivialPreconditioner<AssembledGalerkinOperator<Assembler,0,1,0,1> > trivial;
          CG<LinearSpace,LinearSpace> cg(A,trivial,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,1,0,1> > 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,1,0,1> > 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,1,0,1>::range_type, AssembledGalerkinOperator<Assembler,0,1,0,1>::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,1,0,1> >
          BoomerAMGPrecon(A,steps,coarsentype,interpoltype,tol,cycleType,relaxType,
          strongThreshold,variant,overlap,1,verbosity);
          CG<LinearSpace,LinearSpace> cg(A,BoomerAMGPrecon,defaultScalarProduct,termination,verbosity);
//          Dune::LoopSolver<LinearSpace> cg(A,BoomerAMGPrecon,iteEps,iteSteps,verbosity);
          cg.apply(solution,rhs,res);
        }
        break;
        case PrecondType::JACOBI:
        default:
        {
          JacobiPreconditioner<AssembledGalerkinOperator<Assembler,0,1,0,1> > jacobi(A,1.0);
          CG<LinearSpace,LinearSpace> cg(A,jacobi,defaultScalarProduct,termination,verbosity);
          cg.apply(solution,rhs,res);
        }
        break;
      }
      solution *= -1.0;
      x.data = solution.data;

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

	// graphical output of solution
    std::ostringstream fn;
    fn << "graph/peak-grid";
    fn.width(3);
    fn.fill('0');
    fn.setf(std::ios_base::right,std::ios_base::adjustfield);
    fn << refSteps;
    fn.flush();

    // output of solution in VTK format for visualization,
    // the data are written as ascii stream into file temperature.vtu,
    // possible is also binary
    writeVTKFile(x,fn.str(),IoOptions().setOrder(order));

  // 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,x,fn.str(),options);

    VariableSet::VariableSet e = x;
    projectHierarchically(variableSet,e);
    e -= x;

    accurate = embeddedErrorEstimator(variableSet,e,x,IdentityScaling(),tol,gridManager,verbosity);
    nnz = assembler.nnz(0,1,0,1,onlyLowerTriangle);;
    size_t size = variableSet.degreesOfFreedom(0,1);
    if ( verbosity>0) std::cout << "new mesh: nnz = " << nnz << ", dof = " << size << std::endl;

    //ridx.resize(nnz);
    //cidx.resize(nnz);
    //data.resize(nnz);
    //rhs.resize(size);
    //solution.resize(size);

    // VariableSet::VariableSet xx may be used beyond the do...while loop
    xx.data = x.data;
    iter++;
    if (iter>9)
    {
      std::cout << "*** Maximum number of iterations exceeded ***" << std::endl;
      break;
    }

  }  while (!accurate);

  std::cout << "total computing time: " << boost::timer::format(totalTimer.elapsed()) << "\n";
  std::cout << "End heat transfer (peak source) tutorial program" << std::endl;
}


#include <lib_template/dummy.h>

#include <boost/any.hpp>

#include <plog/Log.h>

#include <seqan/graph_algorithms.h>

#include <mpir.h>
#include <boost/multiprecision/gmp.hpp>

#include <sstream>

namespace boost_mp = boost::multiprecision;
using int_type = boost_mp::number<boost_mp::backends::gmp_int,
                                  boost_mp::expression_template_option::et_off>;

using namespace seqan;

using TGraph = Graph<Directed<>>;
using TVertexDescriptor = VertexDescriptor<TGraph>::Type;
using TEdgeIterator = Iterator<TGraph, EdgeIterator>::Type;
using TSize = Size<TGraph>::Type;

Dummy::Dummy()
{
    LOG_INFO << "Dummy lib constructed";

    // Create graph with 9 directed edges (0,1), (0,2)
    TSize numEdges = 9;
    TVertexDescriptor edges[] = {0, 1, 0, 2, 0, 4, 1, 3, 1, 4, 2, 1, 3, 0, 3, 2, 4, 3};
    TGraph g;
    addEdges(g, edges, numEdges);
    // Print graph.
    std::stringstream gs;
    gs << g;
    LOG_INFO << gs.str();

    // Fill external property map with edge weights and assign to graph.
    int_type weights[] = {3, 8, -4, 1, 7, 4, 2, -5, 6};
    String<int_type> weightMap;
    assignEdgeMap(weightMap, g, weights);

    // Run Floyd-Warshall algorithm.
    String<int_type> distMat;
    String<TVertexDescriptor> predMat;
    floydWarshallAlgorithm(distMat, predMat, g, weightMap);

    // Print result to stdout.
    unsigned int len = static_cast<unsigned>(std::sqrt(static_cast<double >(length(distMat))));
    for (TSize row = 0; row < len; ++row)
        for (TSize col = 0; col < len; ++col)
        {
            std::stringstream s;
            s << row << "," << col << " (Distance="
                 <<  getValue(distMat, row * len + col) << "): ";
            LOG_INFO << s.str();
        }

}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_LOG_2OLOG_10_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_LOG_2OLOG_10_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    Generates constant Log_2olog_10 : \f$\frac{\log(2)}{\log(10)}\f$.

    @par Header <boost/simd/constant/log_2olog_10.hpp>

    @par Semantic:

    @code
    T r = Log_2olog_10<T>();
    @endcode

    is similar to:

    @code
      r =  T(0.3010299956639811952137388947244930267681898814621085);
    @endcode

**/
  template<typename T> T Log_2olog_10();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Generates constant Log_2olog_10.  (\f$\frac{\log(2)}{\log(10)}\f$)

      Generate the  constant log_2olog_10.

      @return The Log_2olog_10 constant for the proper type
    **/
    Value Log_2olog_10<Value>();
  }
} }
#endif

#include <boost/simd/constant/scalar/log_2olog_10.hpp>
#include <boost/simd/constant/simd/log_2olog_10.hpp>

#endif


/**
 *
 * Copyright (C) 2021 Mohammad Javad Dousti, Qing Xie, Mahdi Nazemi,
 * and Massoud Pedram. All rights reserved.
 *
 * Please refer to the LICENSE file for terms of use.
 *
 */

#pragma once

#include <Eigen/SparseCore>
#include <Eigen/Core>

#include "device.hpp"
#include "general.hpp"
#include "rc_utils.hpp"
#include "utils.hpp"

/* model specific constants */
/* changed from 1/2 to 1/3 due to the difference from traditional Elmore Delay
 * scenario */
//#define C_FACTOR    0.33       /* fitting factor to match floworks (due to
// lumping)    */

class Model {
private:
  Device *device;

  Eigen::SparseMatrix<VALUE> g_matrix_;

  // A = C^-1 * G
  Eigen::SparseMatrix<VALUE> a_matrix_;

  // Inverted form of diagonal matrix C
  Eigen::DiagonalMatrix<VALUE, Eigen::Dynamic, Eigen::Dynamic> inv_c_;

  /**
   * This saves "device->getTemperature() * K1",
   * which comes from the lhs of the equation due to dropping
   * the term corresponds to the thermal coupling to the ambient.
   */
  Eigen::Matrix<VALUE, Eigen::Dynamic, 1> amb_vector_;
  Eigen::Matrix<VALUE, Eigen::Dynamic, 1> p_vector_;
  Eigen::Matrix<VALUE, Eigen::Dynamic, 1> t_vector_;

  bool p_vector_made_;
  bool g_matrix_made_;
  bool c_vector_made_;
  bool t_vector_made_;
  bool transient_;
  int elements_no_;
  unordered_map<string, int> powerMappingDeviceOrder;
  unordered_map<int, string> powerMappingTraceOrder;
  std::ifstream powerTraceFile;
  bool isComment(string s);
  /**
   * This designates the number of power consumers we
   * expect to find in the power trace file It is used to
   * check if the power trace has enough info
   */
  int pwr_consumers_cnt;
  string powerTraceFileAddr;

  void initPowerVector();
  void preparePVector();

public:
  void makePVector();
  void makeResistanceModel();
  // void makeResistanceModel2();
  void makeCapacitanceModel();

  /**
   * This function reads one line from the power trace file and place it
   * in the power vector
   *
   * @return Number of succesfully read power values from the last read row.
   */
  unsigned read_power();

  void solveSteadyState();
  void solveTransientState();
  void printSubComponentTemp();
  void printComponentTemp(string file_output);
  void printComponentTemp(string file_output, unsigned step_no);
  void printGMatrix(string file_output);
  void printAMatrix(string file_output);
  void printInvCVector(string file_output);
  void printPVector(string file_output);
  void printTVector(string file_output);
  void printElementCount(string file_output);
  Model(Device *device, bool isTransient);
  virtual ~Model();
};


//  (C) Copyright Jeremy William Murphy 2016.

//  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_MATH_TOOLS_POLYNOMIAL_GCD_HPP
#define BOOST_MATH_TOOLS_POLYNOMIAL_GCD_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/tools/polynomial.hpp>
#include <boost/math/common_factor_rt.hpp>
#include <boost/type_traits/is_pod.hpp>

namespace boost{

   namespace integer {

      namespace gcd_detail {

         template <class T>
         struct gcd_traits;

         template <class T>
         struct gcd_traits<boost::math::tools::polynomial<T> >
         {
            inline static const boost::math::tools::polynomial<T>& abs(const boost::math::tools::polynomial<T>& val) { return val; }

            static const method_type method = method_euclid;
         };

      }
}

namespace math{ namespace tools{

/* From Knuth, 4.6.1:
*
* We may write any nonzero polynomial u(x) from R[x] where R is a UFD as
*
*      u(x) = cont(u) · pp(u(x))
*
* where cont(u), the content of u, is an element of S, and pp(u(x)), the primitive
* part of u(x), is a primitive polynomial over S.
* When u(x) = 0, it is convenient to define cont(u) = pp(u(x)) = O.
*/

template <class T>
T content(polynomial<T> const &x)
{
    return x ? gcd_range(x.data().begin(), x.data().end()).first : T(0);
}

// Knuth, 4.6.1
template <class T>
polynomial<T> primitive_part(polynomial<T> const &x, T const &cont)
{
    return x ? x / cont : polynomial<T>();
}

template <class T>
polynomial<T> primitive_part(polynomial<T> const &x)
{
    return primitive_part(x, content(x));
}

// Trivial but useful convenience function referred to simply as l() in Knuth.
template <class T>
T leading_coefficient(polynomial<T> const &x)
{
    return x ? x.data().back() : T(0);
}

namespace detail
{
    /* Reduce u and v to their primitive parts and return the gcd of their
    * contents. Used in a couple of gcd algorithms.
    */
    template <class T>
    T reduce_to_primitive(polynomial<T> &u, polynomial<T> &v)
    {
        using boost::math::gcd;
        T const u_cont = content(u), v_cont = content(v);
        u /= u_cont;
        v /= v_cont;
        return gcd(u_cont, v_cont);
    }
}

/**
* Knuth, The Art of Computer Programming: Volume 2, Third edition, 1998
* Algorithm 4.6.1C: Greatest common divisor over a unique factorization domain.
*
* The subresultant algorithm by George E. Collins [JACM 14 (1967), 128-142],
* later improved by W. S. Brown and J. F. Traub [JACM 18 (1971), 505-514].
*
* Although step C3 keeps the coefficients to a "reasonable" size, they are
* still potentially several binary orders of magnitude larger than the inputs.
* Thus, this algorithm should only be used where T is a multi-precision type.
*
* @tparam   T   Polynomial coefficient type.
* @param    u   First polynomial.
* @param    v   Second polynomial.
* @return       Greatest common divisor of polynomials u and v.
*/
template <class T>
typename enable_if_c< std::numeric_limits<T>::is_integer, polynomial<T> >::type
subresultant_gcd(polynomial<T> u, polynomial<T> v)
{
    using std::swap;
    BOOST_ASSERT(u || v);

    if (!u)
        return v;
    if (!v)
        return u;

    typedef typename polynomial<T>::size_type N;

    if (u.degree() < v.degree())
        swap(u, v);

    T const d = detail::reduce_to_primitive(u, v);
    T g = 1, h = 1;
    polynomial<T> r;
    while (true)
    {
        BOOST_ASSERT(u.degree() >= v.degree());
        // Pseudo-division.
        r = u % v;
        if (!r)
            return d * primitive_part(v); // Attach the content.
        if (r.degree() == 0)
            return d * polynomial<T>(T(1)); // The content is the result.
        N const delta = u.degree() - v.degree();
        // Adjust remainder.
        u = v;
        v = r / (g * detail::integer_power(h, delta));
        g = leading_coefficient(u);
        T const tmp = detail::integer_power(g, delta);
        if (delta <= N(1))
            h = tmp * detail::integer_power(h, N(1) - delta);
        else
            h = tmp / detail::integer_power(h, delta - N(1));
    }
}

/**
 * @brief GCD for polynomials with unbounded multi-precision integral coefficients.
 *
 * The multi-precision constraint is enforced via numeric_limits.
 *
 * Note that intermediate terms in the evaluation can grow arbitrarily large, hence the need for
 * unbounded integers, otherwise numeric loverflow would break the algorithm.
 *
 * @tparam  T   A multi-precision integral type.
 */
template <typename T>
typename enable_if_c<std::numeric_limits<T>::is_integer && !std::numeric_limits<T>::is_bounded, polynomial<T> >::type
gcd(polynomial<T> const &u, polynomial<T> const &v)
{
    return subresultant_gcd(u, v);
}
// GCD over bounded integers is not currently allowed:
template <typename T>
typename enable_if_c<std::numeric_limits<T>::is_integer && std::numeric_limits<T>::is_bounded, polynomial<T> >::type
gcd(polynomial<T> const &u, polynomial<T> const &v)
{
   BOOST_STATIC_ASSERT_MSG(sizeof(v) == 0, "GCD on polynomials of bounded integers is disallowed due to the excessive growth in the size of intermediate terms.");
   return subresultant_gcd(u, v);
}
// GCD over polynomials of floats can go via the Euclid algorithm:
template <typename T>
typename enable_if_c<!std::numeric_limits<T>::is_integer && (std::numeric_limits<T>::min_exponent != std::numeric_limits<T>::max_exponent) && !std::numeric_limits<T>::is_exact, polynomial<T> >::type
gcd(polynomial<T> const &u, polynomial<T> const &v)
{
   return boost::integer::gcd_detail::Euclid_gcd(u, v);
}

}
//
// Using declaration so we overload the default implementation in this namespace:
//
using boost::math::tools::gcd;

}

namespace integer
{
   //
   // Using declaration so we overload the default implementation in this namespace:
   //
   using boost::math::tools::gcd;
}

} // namespace boost::math::tools

#endif


#include "problemes.h"
#include "arithmetique.h"

#include <fstream>
#include <boost/algorithm/string.hpp>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;

namespace {
    typedef std::vector<std::pair<std::string, nombre>> S;
    typedef std::map<nombre, std::set<S>> C;

    C construire(const S &score, const C &combinaisons) {
        auto suivant = combinaisons;
        for (const auto &s: score) {
            for (const auto &r: combinaisons) {
                auto c = r.second;
                for (auto f: c) {
                    f.push_back(s);
                    std::sort(f.begin(), f.end());
                    suivant[s.second + r.first].insert(f);
                }
            }
        }
        return suivant;
    };
}

ENREGISTRER_PROBLEME(109, "Darts") {
    // In the game of darts a player throws three darts at a target board which is split into twenty equal sized
    // sections numbered one to twenty.
    //
    // The score of a dart is determined by the number of the region that the dart lands in. A dart landing outside the
    // red/green outer ring scores zero. The black and cream regions inside this
    // ring represent single scores. However, the red/green outer ring and middle ring score double and treble scores
    // respectively.
    //
    // At the centre of the board are two concentric circles called the bull region, or bulls-eye.
    // The outer bull is worth 25 points and the inner bull is a double, worth 50 points.
    //
    // There are many variations of rules but in the most popular game the players will begin with a score 301 or 501
    // and the first player to reduce their running total to zero is a winner.
    // However, it is normal to play a "doubles out" system, which means that the player must land a double (including
    // the double bulls-eye at the centre of the board) on their final dart to win;
    // any other dart that would reduce their running total to one or lower means the score for that set of three darts
    // is "bust".
    //
    // When a player is able to finish on their current score it is called a "checkout" and the highest checkout is 170:
    // T20 T20 D25 (two treble 20s and double bull).
    //
    // There are exactly eleven distinct ways to checkout on a score of 6:
    //
    //      D3
    //      D1	D2
    //      S2	D2
    //      D2	D1
    //      S4	D1
    //      S1	S1	D2
    //      S1	T1	D1
    //      S1	S3	D1
    //      D1	D1	D1
    //      D1	S2	D1
    //      S2	S2	D1
    //
    // Note that D1 D2 is considered different to D2 D1 as they finish on different doubles. However, the combination
    // S1 T1 D1 is considered the same as T1 S1 D1.
    //
    // In addition we shall not include misses in considering combinations; for example, D3 is the same as 0 D3 and
    // 0 0 D3.
    //
    // Incredibly there are 42336 distinct ways of checking out in total.
    //
    // How many distinct ways can a player checkout with a score less than 100?
    S score;
    S score_double;

    for (nombre n = 1; n < 21; ++n) {
        score.emplace_back(utilitaires::concatener("S", n), n);
        score.emplace_back(utilitaires::concatener("D", n), 2 * n);
        score.emplace_back(utilitaires::concatener("T", n), 3 * n);
        score_double.emplace_back(utilitaires::concatener("D", n), 2 * n);
    }

    score.emplace_back(utilitaires::concatener("S", 25), 25);
    score.emplace_back(utilitaires::concatener("D", 25), 2 * 25);
    score_double.emplace_back(utilitaires::concatener("D", 25), 2 * 25);

    const S zero{std::make_pair("0", 0)};
    C combinaisons;
    combinaisons[0].insert(zero);

    combinaisons = construire(score, combinaisons);
    combinaisons = construire(score, combinaisons);

    C solution;
    for (const auto &s: score_double) {
        for (const auto &r: combinaisons) {
            auto c = r.second;
            for (auto f: c) {
                f.push_back(s);
                solution[s.second + r.first].insert(f);
            }
        }
    }

    nombre resultat = 0;
    for (const auto &r: solution) {
        if (r.first < 100)
            resultat += r.second.size();
    }

    return std::to_string(resultat);
}


#include <iostream>
#include <math.h>
#include "Vector.h"
#include <boost/multiprecision/gmp.hpp>

using namespace std;

int target = 1000;

int main(int argc, char** argv) {
  boost::multiprecision::mpz_int min = 1;
  for (int i = 1; i < target; i++) {
    min *= 10;
  }
  Vector<boost::multiprecision::mpz_int> fibonacci;
  fibonacci.insertBack(1);
  fibonacci.insertBack(1);
  while (fibonacci[fibonacci.length() - 1] < min) {
    int end = fibonacci.length() - 1;
    fibonacci.insertBack(fibonacci[end] + fibonacci[end - 1]);
  }
  cout << "Index of first Fibonacci number with " << target << " digits: " << fibonacci.length() << endl;
  return 0;
}


/*
 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/

#include <Eigen/Dense>

#include "fvElement.h"

#include "fvElementBuilder.h"

namespace Vitelotte
{

template < class _Mesh, typename _Scalar >
FVElementBuilder<_Mesh, _Scalar>::FVElementBuilder(Scalar sigma)
  : m_sigma(sigma)
{
}

template < class _Mesh, typename _Scalar >
unsigned
FVElementBuilder<_Mesh, _Scalar>::
    nCoefficients(const Mesh& mesh, Face element,
                  SolverError* /*error*/) const
{
    return mesh.nVertexGradientConstraints(element)? 61: 45;
}

template < class _Mesh, typename _Scalar >
unsigned
FVElementBuilder<_Mesh, _Scalar>::
    nExtraConstraints(const Mesh& mesh, Face element) const
{
    return mesh.nVertexGradientConstraints(element)? 2: 0;
}

template < class _Mesh, typename _Scalar >
template < typename Inserter >
void
FVElementBuilder<_Mesh, _Scalar>::
    addCoefficients(Inserter& inserter, const Mesh& mesh,
                    Face element, SolverError* error)
{
    if(mesh.valence(element) != 3)
    {
        if(error) error->error("Non-triangular face");
        return;
    }

    typedef Eigen::Matrix<Scalar, 9, 9> Matrix9;
    Matrix9 sm;

    int nodes[9];

    typename Mesh::HalfedgeAroundFaceCirculator hit = mesh.halfedges(element);
    typename Mesh::HalfedgeAroundFaceCirculator hend = hit;
    do ++hit;
    while(!mesh.isGradientConstraint(mesh.toVertex(*hit)) && hit != hend);
    bool isPgc = mesh.isGradientConstraint(mesh.toVertex(*hit));

    bool orient[3];
    // TODO: remove dynamic allocation with dynamic dims.
    Vector p[3];
    --hit;
    for(int i = 0; i < 3; ++i)
    {
        orient[i] = mesh.halfedgeOrientation(*hit);
        nodes[3+i] = mesh.edgeValueNode(*hit).idx();
        nodes[6+i] = mesh.edgeGradientNode(*hit).idx();
        ++hit;
        nodes[i] = mesh.toVertexValueNode(*hit).idx();
        p[i] = mesh.position(mesh.toVertex(*hit)).template cast<Scalar>();
    }

    for(int i = 0; i < 9; ++i)
    {
        if(nodes[i] < 0)
        {
            if(error) error->error("Invalid node");
            return;
        }
    }

    typedef FVElement<Scalar> Elem;
    Elem elem(p);

    if(elem.doubleArea() <= 0)
    {
        if(error) error->warning("Degenerated or reversed triangle");
    }

    typedef Eigen::Array<Scalar, 3, 1> Array3;
    Array3 dx2[9];
    Array3 dy2[9];
    Array3 dxy[9];
    for(int pi = 0; pi < 3; ++pi)
    {
        Vector3 bc((pi == 0)? 0: .5,
                   (pi == 1)? 0: .5,
                   (pi == 2)? 0: .5);
        typename Elem::Hessian hessians[9];
        elem.hessian(bc, hessians);

        for(int bi = 0; bi < 9; ++bi)
        {
            dx2[bi](pi) = hessians[bi](0, 0);
            dy2[bi](pi) = hessians[bi](1, 1);
            dxy[bi](pi) = hessians[bi](0, 1);
        }
    }

    for(size_t i = 0; i < 9; ++i)
    {
        for(size_t j = i; j < 9; ++j)
        {
            EIGEN_ASM_COMMENT("MYBEGIN");

            Array3 quadPointValue =
                    (dx2[i]+dy2[i]) * (dx2[j]+dy2[j])
                  + (1.-m_sigma) * (
                        2. * dxy[i] * dxy[j]
                      - dx2[i] * dy2[j]
                      - dx2[j] * dy2[i]);

            Scalar value = quadPointValue.sum() * (elem.doubleArea() / 6);

            EIGEN_ASM_COMMENT("MYEND");

            if((i < 6 || orient[i%3]) != (j < 6 || orient[j%3]))
            {
                value *= -1;
            }

            sm(i, j) = value;
            sm(j, i) = value;
        }
    }

    for(size_t i = 0; i < 9; ++i)
    {
        for(size_t j = 0; j < 9; ++j)
        {
            if(nodes[i] < nodes[j]) continue;
            inserter.addCoeff(nodes[i], nodes[j], sm(i, j));
        }
    }

    if(isPgc)
    {
        typedef Eigen::Matrix<Scalar, 9, 1> Vector9;
        Vector9 fde1, fde2;
        fde1 <<
            -1.0L/2.0L*(elem.doubleArea()*(2*elem.dldn(0, 1) + elem.dldn(1, 1)) + 7*elem.edgeLength(1))/(elem.edgeLength(1)*elem.edgeLength(2)),
            (1.0L/2.0L)*(elem.doubleArea()*(elem.dldn(0, 0) + 2*elem.dldn(1, 0)) - elem.edgeLength(0))/(elem.edgeLength(0)*elem.edgeLength(2)),
            -1.0L/2.0L*elem.doubleArea()*(elem.edgeLength(0)*(elem.dldn(1, 1) + 2*elem.dldn(2, 1)) - elem.edgeLength(1)*(elem.dldn(0, 0) + 2*elem.dldn(2, 0)))/(elem.edgeLength(0)*elem.edgeLength(1)*elem.edgeLength(2)),
            -4/elem.edgeLength(2),
            4/elem.edgeLength(2),
            4/elem.edgeLength(2),
            elem.doubleArea()/(elem.edgeLength(0)*elem.edgeLength(2)),
            -elem.doubleArea()/(elem.edgeLength(1)*elem.edgeLength(2)),
            0;
        fde2 <<
            -1.0L/2.0L*(elem.doubleArea()*(2*elem.dldn(0, 2) + elem.dldn(2, 2)) + 7*elem.edgeLength(2))/(elem.edgeLength(1)*elem.edgeLength(2)),
            -1.0L/2.0L*elem.doubleArea()*(elem.edgeLength(0)*(2*elem.dldn(1, 2) + elem.dldn(2, 2)) - elem.edgeLength(2)*(elem.dldn(0, 0) + 2*elem.dldn(1, 0)))/(elem.edgeLength(0)*elem.edgeLength(1)*elem.edgeLength(2)),
            (1.0L/2.0L)*(elem.doubleArea()*(elem.dldn(0, 0) + 2*elem.dldn(2, 0)) - elem.edgeLength(0))/(elem.edgeLength(0)*elem.edgeLength(1)),
            -4/elem.edgeLength(1),
            4/elem.edgeLength(1),
            4/elem.edgeLength(1),
            elem.doubleArea()/(elem.edgeLength(0)*elem.edgeLength(1)),
            0,
            -elem.doubleArea()/(elem.edgeLength(1)*elem.edgeLength(2));

        for(size_t i = 0; i < 9; ++i)
        {
            Scalar f = (i < 6 || orient[i%3])? 1: -1;
            if(i != 8 /*fde1(i) != Scalar(0.)*/)
            {
                inserter.addExtraCoeff(element, 1, nodes[i], fde1(i) * f);
            }
            if(i != 7 /*fde2(i) != Scalar(0.)*/)
            {
                inserter.addExtraCoeff(element, 0, nodes[i], fde2(i) * f);
            }
        }
    }
}

template < class _Mesh, typename _Scalar >
template < typename Inserter >
void
FVElementBuilder<_Mesh, _Scalar>::
    addExtraConstraints(Inserter& inserter, const Mesh& mesh,
                                Face element, SolverError* /*error*/)
{
    typename Mesh::HalfedgeAroundFaceCirculator hit = mesh.halfedges(element);
    typename Mesh::HalfedgeAroundFaceCirculator hend = hit;
    do ++hit;
    while(!mesh.isGradientConstraint(mesh.toVertex(*hit)) && hit != hend);
    if(!mesh.isGradientConstraint(mesh.toVertex(*hit))) {
        return;
    }

    for(unsigned hi = 0; hi < 2; ++hi)
    {
        typename Mesh::Halfedge h = *hit;

        typename Mesh::Vertex from = mesh.fromVertex(h);
        typename Mesh::Vertex to   = mesh.  toVertex(h);

        bool v0c = mesh.isGradientConstraint(from);
        const typename Mesh::Gradient& grad = mesh.gradientConstraint(v0c? from: to);
        typename Mesh::Vector v = mesh.position(to) - mesh.position(from);
        if(!v0c) v = -v;
        typename Mesh::Value cons = grad * v;
        inserter.setExtraRhs(element, hi, cons.template cast<Scalar>());

        ++hit;
    }

}

}


#include <iostream>
#include <cmath>
#include <chrono>
#include <limits>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/core/eigen.hpp>

#include "pose2d.h"
#include "Ransac.hh"
#include "PoseRANSAC.hh"

//#define USE_QUATERNION
#define USE_ROTATION_MATRIX

namespace pose2d
{
/* Matrix coefficients for the non-depth case matrix to be solved by SVD */
   inline double tx_coeff(double xp1, double yp1, double xq1, double yq1, const Eigen::Matrix3d& R)
//---------------------------------------------------------------------------------------------------
   {
      return (R(1, 2) + R(1, 0) * xp1 + R(1, 1) * yp1 - R(2, 2) * yq1 + (-(R(2, 0) * xp1) - R(2, 1) * yp1) * yq1);
      //return (R(1,2) + R(1,0)*xq1 - R(2,2)*yp1 - R(2,0)*xq1*yp1 + R(1,1)*yq1 - R(2,1)*yp1*yq1);
   }

   inline double ty_coeff(double xp1, double yp1, double xq1, double yq1, const Eigen::Matrix3d& R)
//----------------------------------------------------------------------------------------------------
   {
      return (-R(0, 2) - R(0, 0) * xp1 + R(2, 2) * xq1 + R(2, 0) * xp1 * xq1 - R(0, 1) * yp1 + R(2, 1) * xq1 * yp1);
      //return ((-R(0,2) + R(2,2)*xp1 - R(0,0)*xq1 + R(2,0)*xp1*xq1 - R(0,1)*yq1 + R(2,1)*xp1*yq1));
   }

   inline double tz_coeff(double xp1, double yp1, double xq1, double yq1, const Eigen::Matrix3d& R)
//---------------------------------------------------------------------------------------------------
   {
      return (-(R(1, 2) * xq1) - R(1, 0) * xp1 * xq1 - R(1, 1) * xq1 * yp1 +
              (R(0, 2) + R(0, 0) * xp1 + R(0, 1) * yp1) * yq1);
//   return (-(R(1,2)*xp1) - R(1,0)*xp1*xq1 + R(0,2)*yp1 + R(0,0)*xq1*yp1 - R(1,1)*xp1*yq1 + R(0,1)*yp1*yq1);
   }

/* RHS values for depth solution*/
   inline double b0(const Eigen::Matrix3d& R, const double x_0, const double y_0, const double d_0, const double y_1)
   {
      return d_0 *
             (-R(1, 0) * x_0 - R(1, 1) * y_0 - R(1, 2) + R(2, 0) * x_0 * y_1 + R(2, 1) * y_0 * y_1 + R(2, 2) * y_1);
   }

   inline double b1(const Eigen::Matrix3d& R, const double x_0, const double y_0, const double d_0, const double x_1)
   {
      return -d_0 *
             (-R(0, 0) * x_0 - R(0, 1) * y_0 - R(0, 2) + R(2, 0) * x_0 * x_1 + R(2, 1) * x_1 * y_0 + R(2, 2) * x_1);
   }

   inline double b2(const Eigen::Matrix3d& R, const double x_0, const double y_0, const double d_0,
                    const double x_1, const double y_1)
   {
      return d_0 *
             (-R(0, 0) * x_0 * y_1 - R(0, 1) * y_0 * y_1 - R(0, 2) * y_1 + R(1, 0) * x_0 * x_1 + R(1, 1) * x_1 * y_0 +
              R(1, 2) * x_1);
   }

   inline double b0(double Qw, double Qx, double Qy, double Qz, double x_0, double y_0, double y_1, double d)
   {
      double Qw2 = Qw * Qw, Qx2 = Qx * Qx, Qy2 = Qy * Qy, Qz2 = Qz * Qz, QwQx = Qw * Qx;
      return d * (-Qw2 * y_1 - 2 * QwQx * y_0 * y_1 - 2 * QwQx + 2 * Qw * Qy * x_0 * y_1 + Qx2 * y_1 -
                  2 * Qx * Qz * x_0 * y_1 +
                  Qy2 * y_1 - 2 * Qy * Qz * y_0 * y_1 + 2 * Qy * Qz - Qz2 * y_1 + 2 * x_0 * (Qw * Qz + Qx * Qy) +
                  y_0 * (Qw2 -
                         Qx2 + Qy2 - Qz2));
   }

   inline double b1(double Qw, double Qx, double Qy, double Qz, double x_0, double y_0, double x_1, double d)
   {
      double Qw2 = Qw * Qw, Qx2 = Qx * Qx, Qy2 = Qy * Qy, Qz2 = Qz * Qz, QwQy = Qw * Qy, QxQz = Qx * Qz;
      return -d *
             (-Qw2 * x_1 - 2 * Qw * Qx * x_1 * y_0 + 2 * QwQy * x_0 * x_1 + 2 * QwQy - 2 * Qw * Qz * y_0 + Qx2 * x_1 +
              2 * Qx * Qy * y_0 - 2 * QxQz * x_0 * x_1 + 2 * QxQz + Qy2 * x_1 - 2 * Qy * Qz * x_1 * y_0 - Qz2 * x_1 +
              x_0 * (Qw2 + Qx2 - Qy2 - Qz2));
   }

   inline double b2(double Qw, double Qx, double Qy, double Qz, double x_0, double y_0, double x_1, double y_1, double d)
   {
      double Qw2 = Qw * Qw, Qx2 = Qx * Qx, Qy2 = Qy * Qy, Qz2 = Qz * Qz, QwQz = Qw * Qz, QxQy = Qx * Qy;
      return -d *
             (-Qw2 * x_0 * y_1 - 2 * Qw * Qx * x_1 - 2 * Qw * Qy * y_1 + 2 * QwQz * x_0 * x_1 + 2 * QwQz * y_0 * y_1 -
              Qx2 * x_0 * y_1 + 2 * QxQy * x_0 * x_1 - 2 * QxQy * y_0 * y_1 - 2 * Qx * Qz * y_1 + Qy2 * x_0 * y_1 +
              2 * Qy * Qz * x_1 + Qz2 * x_0 * y_1 + x_1 * y_0 * (Qw2 - Qx2 + Qy2 - Qz2));

   }

   inline Eigen::Vector3d homogeneous(Eigen::Matrix3d A)
   //---------------------------------------------------------------------------------------------------------------------------
   {
      Eigen::JacobiSVD<Eigen::MatrixXd, Eigen::FullPivHouseholderQRPreconditioner>
            svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
      auto U = svd.matrixU();
      auto V = svd.matrixV();
      return V.col(V.cols() - 1);
   }

   inline Eigen::Quaterniond rotation(const Eigen::Vector3d &from, const Eigen::Vector3d &to,
                                      const Eigen::Vector3d &fallbackAxis = Eigen::Vector3d(0, 0, 0))
   //-----------------------------------------------------------------------------------------------
   {
      Eigen::Quaterniond q;
      Eigen::Vector3d v0 = from;
      Eigen::Vector3d v1 = to;
      v0.normalize();
      v1.normalize();

      double d = v0.dot(v1);
      if (d >= 1.0f)
         return Eigen::Quaterniond(1, 0, 0, 0);

      if (d < (1e-6f - 1.0f))
      {
         if (fallbackAxis != Eigen::Vector3d(0, 0, 0))
            q = Eigen::AngleAxis<double>(PI, fallbackAxis);
         else
         {
            // Generate an axis
            Eigen::Vector3d axis = Eigen::Vector3d(1, 0, 0).cross(from);
            if (axis.norm() < 0.000000001) // pick another if colinear
               axis = Eigen::Vector3d(0, 1, 0).cross(from);
            axis.normalize();
            q = Eigen::AngleAxis<double>(PI, axis);
         }
      }
      else
      {
         double s = sqrt((1 + d) * 2);
         double invs = 1 / s;

         Eigen::Vector3d c = v0.cross(v1);

         q.x() = c.x() * invs;
         q.y() = c.y() * invs;
         q.z() = c.z() * invs;
         q.w() = s * 0.5f;
         q.normalize();
      }
      return q;
   }

   void pose(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts, const Eigen::Vector3d& train_g,
             const Eigen::Vector3d query_g, const cv::Mat& intrinsics,
             Eigen::Quaterniond& Q, Eigen::Vector3d& translation)
//-----------------------------------------------------------------------------------------------------------
   {
      Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> K((double*) intrinsics.data);
      pose(pts, train_g, query_g, K, Q, translation);
   }

   void pose(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
             const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g,
             const Eigen::Matrix3d& K, Eigen::Quaterniond& Q, Eigen::Vector3d& translation)
   //--------------------------------------------------------------------------------------
   {
      Eigen::Matrix3d KI = K.inverse();
      //   std::cout << K << std::endl << KI << std::endl;
      //   if (std::isnan(KI(0,0))) KI = K;

      Eigen::Vector3d train_gravity(train_g), query_gravity(query_g);
      train_gravity.normalize();
      query_gravity.normalize();
      //   Q = rotation(train_gravity, query_gravity); // camera to model
      Q = rotation(query_gravity, train_gravity); // model to camera
      Eigen::Matrix3d R = Q.toRotationMatrix();
      size_t m = pts.size();
      Eigen::MatrixXd A(m, 3);
      for (size_t row = 0; row < m; row++)
      {
         const cv::Point3d& tpt = pts[row].first;
         const cv::Point3d& qpt = pts[row].second;
         Eigen::Vector3d train_ray = KI*Eigen::Vector3d(tpt.x, tpt.y, 1);
         Eigen::Vector3d query_ray = KI*Eigen::Vector3d(qpt.x, qpt.y, 1);
         double xt1 = train_ray[0], yt1 = train_ray[1], xq1 = query_ray[0], yq1 = query_ray[1];
         A.row(row) << tx_coeff(xt1, yt1, xq1, yq1, R),
               ty_coeff(xt1, yt1, xq1, yq1, R),
               tz_coeff(xt1, yt1, xq1, yq1, R);
      }

      Eigen::JacobiSVD<Eigen::MatrixXd, Eigen::FullPivHouseholderQRPreconditioner>
            svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
      auto V = svd.matrixV();
      translation = V.col(V.cols() - 1);
//   assert(mut::check_essential(R, translation));
   }

   void pose(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
             const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g, const double depth,
             const cv::Mat& intrinsics, Eigen::Quaterniond& Q, Eigen::Vector3d& translation)
//-----------------------------------------------------------------------------------------------------------
   {
      Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> K((double*) intrinsics.data);
      pose(pts, train_g, query_g, depth, K, Q, translation);
   }

   void pose(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
             const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g, const double depth,
             const Eigen::Matrix3d& K, Eigen::Quaterniond& Q, Eigen::Vector3d& translation)
   //--------------------------------------------------------------------------------------
   {
      size_t m = pts.size();
//   if ( (! std::isnan(depth)) && (m > 6 ) )
//   {
//      pose_ransac(pts, train_g, query_g, depth, K, Q, translation, 3);
//      return;
//   }
      Eigen::Matrix3d KI = K.inverse();
      Eigen::Vector3d train_gravity(train_g), query_gravity(query_g);
      train_gravity.normalize();
      query_gravity.normalize();
//   Q = rotation(train_gravity, query_gravity); // camera to model
      Q = rotation(query_gravity, train_gravity); // model to camera

      Eigen::Matrix3d R = Q.toRotationMatrix();
      Eigen::MatrixXd A(3 * m, 3);
      Eigen::VectorXd b(m * 3);
      for (size_t row = 0, ri = 0; row < m; row++)
      {
         const cv::Point3d& tpt = pts[row].first;
         const cv::Point3d& qpt = pts[row].second;
         Eigen::Vector3d train_ray = KI*Eigen::Vector3d(tpt.x, tpt.y, 1);
         Eigen::Vector3d query_ray = KI*Eigen::Vector3d(qpt.x, qpt.y, 1);
         double xt1 = train_ray[0], yt1 = train_ray[1], xq1 = query_ray[0], yq1 = query_ray[1];
#ifdef USE_ROTATION_MATRIX
         A.row(ri) << 0, 1, -yq1;
         b(ri++) = b0(R, xt1, yt1, depth, yq1);
         A.row(ri) << -1, 0, xq1;
         b(ri++) = b1(R, xt1, yt1, depth, xq1);
         A.row(ri) << yq1, -xq1, 0;
         b(ri++) = b2(R, xt1, yt1, depth, xq1, yq1);
#endif
#ifdef USE_QUATERNION
         A.row(ri) << 0, -1, yq1;
         b(ri++) = b0(Q.w(), Q.x(), Q.y(), Q.z(), xt1, yt1, yq1, d);
         A.row(ri) <<  1, 0, -xq1;
         b(ri++) = b1(Q.w(), Q.x(), Q.y(), Q.z(), xt1, yt1, xq1, d);
         A.row(ri) << -yq1, xq1, 0;
         b(ri++) = b2(Q.w(), Q.x(), Q.y(), Q.z(), xt1, yt1, xq1, yq1, d);
#endif
      }
   std::cout << "Rank " << A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).rank() << std::endl;
//      Eigen::ColPivHouseholderQR<Eigen::MatrixXd> MQR(A);
//      translation = MQR.solve(b);
   translation = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
   }

   double pose_ransac(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
                      const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g,
                      const cv::Mat& intrinsics, Eigen::Quaterniond& Q, Eigen::Vector3d& translation,
                      void* RANSAC_params, int samples)
   //------------------------------------------------------------------------------------------------
   {
      Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> K((double*) intrinsics.data);
      return pose_ransac(pts, train_g, query_g, K, Q, translation, RANSAC_params, samples);
   }

   double pose_ransac(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
                      const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g,
                      const Eigen::Matrix3d& K, Eigen::Quaterniond& Q, Eigen::Vector3d& translation,
                      void* RANSAC_params, int samples)
   //-----------------------------------------------------------------------------------------------
   {
      if (RANSAC_params == nullptr) throw std::logic_error("pose2d::pose_ransac (no depth): RANSAC params are null");
      Eigen::Vector3d train_gravity(train_g), query_gravity(query_g);
      train_gravity.normalize();
      query_gravity.normalize();
//   Q = rotation(train_gravity, query_gravity); // camera to model
      Q = rotation(query_gravity, train_gravity); // model to camera
      double confidence = -1;
#ifdef USE_THEIA_RANSAC
      Grav2DRansacEstimator estimator(K, Q, -1, samples);
      theia::RansacParameters* parameters = static_cast<theia::RansacParameters*>(RANSAC_params);
      theia::RansacSummary summary;
      std::unique_ptr<theia::SampleConsensusEstimator<Grav2DRansacEstimator>> ransac =
            theia::CreateAndInitializeRansacVariant(theia::RansacType::RANSAC, *parameters, estimator);
      if (ransac)
      {
         GravPoseRansacModel best_model;
         ransac->Estimate(pts, &best_model, &summary);
         confidence = summary.confidence;
         if (confidence > 0)
         {
            Q = best_model.rotation;
            translation = best_model.translation;
         }
      }
#else
      templransac::RANSACParams* parameters = static_cast<templransac::RANSACParams*>(RANSAC_params);
      Grav2DRansacEstimator estimator(K, Q);
      Grav2DRansacData data(pts);
      std::vector<std::pair<double, GravPoseRansacModel> > results;
      std::vector<std::vector<size_t>> inlier_indices;
      std::stringstream errs;
      confidence = templransac::RANSAC(*parameters, estimator, data, pts.size(), samples, 1,
                                       results, inlier_indices, &errs);
      if (confidence > 0)
      {
         GravPoseRansacModel& model = results[0].second;
         translation = model.translation;
      }
#endif
      return confidence;
   }

   double pose_ransac(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
                      const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g,
                      const double depth, const cv::Mat& intrinsics,
                      Eigen::Quaterniond& Q, Eigen::Vector3d& translation,
                      void* RANSAC_params, int samples)
   //----------------------------------------------------------------------------------------------------------
   {
      Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> K((double*) intrinsics.data);
      return pose_ransac(pts, train_g, query_g, depth, K, Q, translation, RANSAC_params, samples);
   }

   double pose_ransac(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
                      const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g,
                      const double depth, const Eigen::Matrix3d& K,
                      Eigen::Quaterniond& Q, Eigen::Vector3d& translation,
                      void *RANSAC_params, int samples)
   //---------------------------------------------------------------------------------
   {
      if (RANSAC_params == nullptr) throw std::logic_error("pose2d::pose_ransac (with depth): RANSAC params are null");
      Eigen::Vector3d train_gravity(train_g), query_gravity(query_g);
      train_gravity.normalize();
      query_gravity.normalize();
//   Q = rotation(train_gravity, query_gravity); // camera to model
      Q = rotation(query_gravity, train_gravity); // model to camera
      double confidence = -1;
#ifdef USE_THEIA_RANSAC
      Grav2DRansacEstimator estimator(K, Q, depth, samples);
      theia::RansacParameters* parameters = static_cast<theia::RansacParameters*>(RANSAC_params);
      theia::RansacSummary summary;
      std::unique_ptr<theia::SampleConsensusEstimator<Grav2DRansacEstimator>> ransac =
            theia::CreateAndInitializeRansacVariant(theia::RansacType::RANSAC, *parameters, estimator);
      if (ransac)
      {
         GravPoseRansacModel best_model;

         if (ransac->Estimate(pts, &best_model, &summary))
         {
            confidence = summary.confidence;
            if (confidence > 0)
            {
               Q = best_model.rotation;
               translation = best_model.translation;
            }
         }
      }
#else
      templransac::RANSACParams* parameters = static_cast<templransac::RANSACParams*>(RANSAC_params);
      Grav2DDepthRansacEstimator estimator(K, Q, depth);
      Grav2DRansacData data(pts);
      std::vector<std::pair<double, GravPoseRansacModel>> results;
      std::vector<std::vector<size_t>> inlier_indices;
      std::stringstream errs;
      confidence = templransac::RANSAC(*parameters, estimator, data, pts.size(), samples, 1,
                                       results, inlier_indices, &errs);
      if (confidence > 0)
      {
         GravPoseRansacModel& model = results[0].second;
         translation = model.translation;

//      for (size_t k=0; k<results.size(); k++)
//      {
//         std::pair<double, Grav2DRansacModel> pp = results[k];
//         std::cout << "RANSAC Result " << pp.second.translation.transpose() << " ";
//         std::vector<size_t> inliers = inlier_indices[k];
//         for (size_t inlier : inliers)
//            std::cout << train_img_pts[inlier] << " -> " << query_image_pts[inlier] << " | ";
//         std::cout << std::endl;
//      }
      }
#endif
      return confidence;
   }

   void pose_translation(const Eigen::Matrix3d& Kinv, const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
                         const Eigen::Matrix3d& R, Eigen::Vector3d& translation)
//----------------------------------------------------------------------------------------------------------------
   {
      const cv::Point3d& tpt0 = pts[0].first;
      const cv::Point3d& qpt0 = pts[0].second;
      const cv::Point3d& tpt1 = pts[1].first;
      const cv::Point3d& qpt1 = pts[1].second;
      const cv::Point3d& tpt2 = pts[2].first;
      const cv::Point3d& qpt2 = pts[2].second;
      Eigen::Vector3d train_ray1 = Kinv*Eigen::Vector3d(tpt0.x, tpt0.y, 1);
      Eigen::Vector3d query_ray1 = Kinv*Eigen::Vector3d(qpt0.x, qpt0.y, 1);
      Eigen::Vector3d train_ray2 = Kinv*Eigen::Vector3d(tpt1.x, tpt1.y, 1);
      Eigen::Vector3d query_ray2 = Kinv*Eigen::Vector3d(qpt1.x, qpt1.y, 1);
      Eigen::Vector3d train_ray3 = Kinv*Eigen::Vector3d(tpt2.x, tpt2.y, 1);
      Eigen::Vector3d query_ray3 = Kinv*Eigen::Vector3d(qpt2.x, qpt2.y, 1);
      double xt1 = train_ray1[0], yt1 = train_ray1[1], xq1 = query_ray1[0], yq1 = query_ray1[1];
      double xt2 = train_ray2[0], yt2 = train_ray2[1], xq2 = query_ray2[0], yq2 = query_ray2[1];
      double xt3 = train_ray3[0], yt3 = train_ray3[1], xq3 = query_ray3[0], yq3 = query_ray3[1];
      Eigen::Matrix3d A;
      A << tx_coeff(xt1, yt1, xq1, yq1, R),
            ty_coeff(xt1, yt1, xq1, yq1, R),
            tz_coeff(xt1, yt1, xq1, yq1, R),

            tx_coeff(xt2, yt2, xq2, yq2, R),
            ty_coeff(xt2, yt2, xq2, yq2, R),
            tz_coeff(xt2, yt2, xq2, yq2, R),

            tx_coeff(xt3, yt3, xq3, yq3, R),
            ty_coeff(xt3, yt3, xq3, yq3, R),
            tz_coeff(xt3, yt3, xq3, yq3, R);
      translation = homogeneous(A);
   }

   //Called by RANSAC estimation
   void pose_translation(const Eigen::Matrix3d& Kinv, const std::vector<cv::Point3d>& train_img_pts,
                         const std::vector<cv::Point3d>& query_img_pts,
                         const Eigen::Matrix3d& R, Eigen::Vector3d& translation)
//------------------------------------------------------------------------
   {
      const cv::Point3d& tpt0 = train_img_pts[0];
      const cv::Point3d& tpt1 = train_img_pts[1];
      const cv::Point3d& tpt2 = train_img_pts[2];
      const cv::Point3d& qpt0 = query_img_pts[0];
      const cv::Point3d& qpt1 = query_img_pts[1];
      const cv::Point3d& qpt2 = query_img_pts[2];
      Eigen::Vector3d train_ray1 = Kinv*Eigen::Vector3d(tpt0.x, tpt0.y, 1);
      Eigen::Vector3d query_ray1 = Kinv*Eigen::Vector3d(qpt0.x, qpt0.y, 1);
      Eigen::Vector3d train_ray2 = Kinv*Eigen::Vector3d(tpt1.x, tpt1.y, 1);
      Eigen::Vector3d query_ray2 = Kinv*Eigen::Vector3d(qpt1.x, qpt1.y, 1);
      Eigen::Vector3d train_ray3 = Kinv*Eigen::Vector3d(tpt2.x, tpt2.y, 1);
      Eigen::Vector3d query_ray3 = Kinv*Eigen::Vector3d(qpt2.x, qpt2.y, 1);
      double xt1 = train_ray1[0], yt1 = train_ray1[1], xq1 = query_ray1[0], yq1 = query_ray1[1];
      double xt2 = train_ray2[0], yt2 = train_ray2[1], xq2 = query_ray2[0], yq2 = query_ray2[1];
      double xt3 = train_ray3[0], yt3 = train_ray3[1], xq3 = query_ray3[0], yq3 = query_ray3[1];
      Eigen::Matrix3d A;
      A << tx_coeff(xt1, yt1, xq1, yq1, R),
            ty_coeff(xt1, yt1, xq1, yq1, R),
            tz_coeff(xt1, yt1, xq1, yq1, R),

            tx_coeff(xt2, yt2, xq2, yq2, R),
            ty_coeff(xt2, yt2, xq2, yq2, R),
            tz_coeff(xt2, yt2, xq2, yq2, R),

            tx_coeff(xt3, yt3, xq3, yq3, R),
            ty_coeff(xt3, yt3, xq3, yq3, R),
            tz_coeff(xt3, yt3, xq3, yq3, R);
      translation = homogeneous(A);
   }

   void pose_translation(const Eigen::Matrix3d& Kinv, const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts, const double depth,
                         const Eigen::Matrix3d& R, Eigen::Vector3d& translation)
   //-------------------------------------------------------------------------
   {
      const cv::Point3d& tpt0 = pts[0].first;
      const cv::Point3d& qpt0 = pts[0].second;
      const cv::Point3d& tpt1 = pts[1].first;
      const cv::Point3d& qpt1 = pts[1].second;
      const cv::Point3d& tpt2 = pts[2].first;
      const cv::Point3d& qpt2 = pts[2].second;
      Eigen::Vector3d train_ray1 = Kinv*Eigen::Vector3d(tpt0.x, tpt0.y, 1);
      Eigen::Vector3d query_ray1 = Kinv*Eigen::Vector3d(qpt0.x, qpt0.y, 1);
      Eigen::Vector3d train_ray2 = Kinv*Eigen::Vector3d(tpt1.x, tpt1.y, 1);
      Eigen::Vector3d query_ray2 = Kinv*Eigen::Vector3d(qpt1.x, qpt1.y, 1);
      Eigen::Vector3d train_ray3 = Kinv*Eigen::Vector3d(tpt2.x, tpt2.y, 1);
      Eigen::Vector3d query_ray3 = Kinv*Eigen::Vector3d(qpt2.x, qpt2.y, 1);
      double xt1 = train_ray1[0], yt1 = train_ray1[1], xq1 = query_ray1[0], yq1 = query_ray1[1];
      double xt2 = train_ray2[0], yt2 = train_ray2[1], xq2 = query_ray2[0], yq2 = query_ray2[1];
      double xt3 = train_ray3[0], yt3 = train_ray3[1], xq3 = query_ray3[0], yq3 = query_ray3[1];
//   Eigen::MatrixXd A(9, 3);
//   Eigen::VectorXd b(9);
      Eigen::Matrix<double, 9, 3> A;
      Eigen::Matrix<double, 9, 1> b;
      A << 0, 1, -yq1,
            -1, 0, xq1,
            yq1, -xq1, 0,

            0, 1, -yq2,
            -1, 0, xq2,
            yq2, -xq2, 0,

            0, 1, -yq3,
            -1, 0, xq3,
            yq3, -xq3, 0;

      b << b0(R, xt1, yt1, depth, yq1), b1(R, xt1, yt1, depth, xq1), b2(R, xt1, yt1, depth, xq1, yq1),
            b0(R, xt2, yt2, depth, yq2), b1(R, xt2, yt2, depth, xq2), b2(R, xt2, yt2, depth, xq2, yq2),
            b0(R, xt3, yt3, depth, yq3), b1(R, xt3, yt3, depth, xq3), b2(R, xt3, yt3, depth, xq3, yq3);
//      Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 9, 3>> MQR(A);
//      translation = MQR.solve(b);
      translation = A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b);
//   std::cout << "Rank " << A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).rank() << std::endl;
   }

   //Called by RANSAC estimation
   void pose_translation(const Eigen::Matrix3d& Kinv, const std::vector<cv::Point3d>& train_img_pts,
                         const std::vector<cv::Point3d>& query_img_pts, const double depth,
                         const Eigen::Matrix3d& R, Eigen::Vector3d& translation)
   //-------------------------------------------------------------------------
   {
      const cv::Point3d& tpt0 = train_img_pts[0];
      const cv::Point3d& tpt1 = train_img_pts[1];
      const cv::Point3d& tpt2 = train_img_pts[2];
      const cv::Point3d& qpt0 = query_img_pts[0];
      const cv::Point3d& qpt1 = query_img_pts[1];
      const cv::Point3d& qpt2 = query_img_pts[2];
      Eigen::Vector3d train_ray1 = Kinv*Eigen::Vector3d(tpt0.x, tpt0.y, 1);
      Eigen::Vector3d query_ray1 = Kinv*Eigen::Vector3d(qpt0.x, qpt0.y, 1);
      Eigen::Vector3d train_ray2 = Kinv*Eigen::Vector3d(tpt1.x, tpt1.y, 1);
      Eigen::Vector3d query_ray2 = Kinv*Eigen::Vector3d(qpt1.x, qpt1.y, 1);
      Eigen::Vector3d train_ray3 = Kinv*Eigen::Vector3d(tpt2.x, tpt2.y, 1);
      Eigen::Vector3d query_ray3 = Kinv*Eigen::Vector3d(qpt2.x, qpt2.y, 1);
      double xt1 = train_ray1[0], yt1 = train_ray1[1], xq1 = query_ray1[0], yq1 = query_ray1[1];
      double xt2 = train_ray2[0], yt2 = train_ray2[1], xq2 = query_ray2[0], yq2 = query_ray2[1];
      double xt3 = train_ray3[0], yt3 = train_ray3[1], xq3 = query_ray3[0], yq3 = query_ray3[1];
//   Eigen::MatrixXd A(9, 3);
//   Eigen::VectorXd b(9);
      Eigen::Matrix<double, 9, 3> A;
      Eigen::Matrix<double, 9, 1> b;
      A << 0, 1, -yq1,
            -1, 0, xq1,
            yq1, -xq1, 0,

            0, 1, -yq2,
            -1, 0, xq2,
            yq2, -xq2, 0,

            0, 1, -yq3,
            -1, 0, xq3,
            yq3, -xq3, 0;

      b << b0(R, xt1, yt1, depth, yq1), b1(R, xt1, yt1, depth, xq1), b2(R, xt1, yt1, depth, xq1, yq1),
            b0(R, xt2, yt2, depth, yq2), b1(R, xt2, yt2, depth, xq2), b2(R, xt2, yt2, depth, xq2, yq2),
            b0(R, xt3, yt3, depth, yq3), b1(R, xt3, yt3, depth, xq3), b2(R, xt3, yt3, depth, xq3, yq3);
//      Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 9, 3>> MQR(A);
//      translation = MQR.solve(b);
      translation = A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b);
//   std::cout << "Rank " << A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).rank() << std::endl;
   }
}


#pragma once

#define BOOST_MATH_PROMOTE_DOUBLE_POLICY false
#include "tools.hpp"
#include <Eigen/Dense>
#include <algorithm>
#include <boost/math/distributions.hpp>
#include <boost/math/special_functions/hermite.hpp>
#include <random>
#include <vector>

namespace kde1d {

//! statistical functions
namespace stats {

//! standard normal density
//! @param x evaluation points.
//! @return matrix of pdf values.
inline Eigen::MatrixXd
dnorm(const Eigen::MatrixXd& x)
{
  boost::math::normal dist;
  return x.unaryExpr(
    [&dist](const double& y) { return boost::math::pdf(dist, y); });
};

//! standard normal density
//! @param x evaluation points.
//! @param drv order of the derivative
//! @return matrix of pdf values.
inline Eigen::MatrixXd
dnorm_drv(const Eigen::MatrixXd& x, unsigned drv)
{
  boost::math::normal dist;
  double rt2 = std::sqrt(2);
  return x.unaryExpr([&dist, &drv, &rt2](const double& y) {
    double res = boost::math::pdf(dist, y);
    // boost implementes phsyicist's hermite poly; rescale to probabilist's.
    res *= boost::math::hermite(drv, y / rt2);
    res *= std::pow(0.5, drv * 0.5);
    if (drv % 2)
      res = -res;
    return res;
  });
};

//! standard normal cdf
//! @param x evaluation points.
//! @return matrix of cdf values.
inline Eigen::MatrixXd
pnorm(const Eigen::MatrixXd& x)
{
  boost::math::normal dist;
  return x.unaryExpr(
    [&dist](const double& y) { return boost::math::cdf(dist, y); });
};

//! standard normal quantiles
//! @param x evaluation points.
//! @return matrix of quantiles.
inline Eigen::MatrixXd
qnorm(const Eigen::MatrixXd& x)
{
  boost::math::normal dist;
  return x.unaryExpr(
    [&dist](const double& y) { return boost::math::quantile(dist, y); });
};

//! empirical quantiles
//! @param x data.
//! @param q evaluation points.
//! @return vector of quantiles.
inline Eigen::VectorXd
quantile(const Eigen::VectorXd& x, const Eigen::VectorXd& q)
{
  double n = static_cast<double>(x.size() - 1);
  size_t m = q.size();
  Eigen::VectorXd res(m);

  // map to std::vector and sort
  std::vector<double> x2(x.data(), x.data() + x.size());
  std::sort(x2.begin(), x2.end());

  // linear interpolation (quantile of type 7 in R)
  for (size_t i = 0; i < m; ++i) {
    size_t k = std::floor(n * q(i));
    double p = static_cast<double>(k) / n;
    res(i) = x2[k];
    if (k < n)
      res(i) += (x2[k + 1] - x2[k]) * (q(i) - p) * n;
  }
  return res;
}

//! empirical quantiles
//! @param x data.
//! @param q evaluation points.
//! @param w vector of weights.
//! @return vector of quantiles.
inline Eigen::VectorXd
quantile(const Eigen::VectorXd& x,
         const Eigen::VectorXd& q,
         const Eigen::VectorXd& w)
{
  if (w.size() == 0)
    return quantile(x, q);
  if (w.size() != x.size())
    throw std::runtime_error("x and w must have the same size");
  double n = static_cast<double>(x.size());
  size_t m = q.size();
  Eigen::VectorXd res(m);

  // map to std::vector and sort
  std::vector<size_t> ind(n);
  for (size_t i = 0; i < n; ++i)
    ind[i] = i;
  std::sort(
    ind.begin(), ind.end(), [&x](size_t i, size_t j) { return x(i) < x(j); });

  auto x2 = x;
  auto wcum = w;
  double wacc = 0.0;
  for (size_t i = 0; i < n; ++i) {
    x2(i) = x(ind[i]);
    wcum(i) = wacc;
    wacc += w(ind[i]);
  }

  double wsum = w.sum() - w(ind[n - 1]);
  ;
  for (size_t j = 0; j < m; ++j) {
    size_t i = 1;
    while ((wcum(i) < q(j) * wsum) & (i < n))
      i++;
    res(j) = x2(i - 1);
    if (w(ind[i - 1]) > 1e-30) {
      res(j) +=
        (x2(i) - x2(i - 1)) * (q(j) - wcum(i - 1) / wsum) / w(ind[i - 1]);
    }
  }

  return res;
}

// conditionally equidistant jittering; equivalent to the R implementation:
//   tab <- table(x)
//   noise <- unname(unlist(lapply(tab, function(l) -0.5 + 1:l / (l + 1))))
//   s <- sort(x, index.return = TRUE)
//   return((s$x + noise)[rank(x, ties.method = "first", na.last = "keep")])
inline Eigen::VectorXd
equi_jitter(const Eigen::VectorXd& x)
{
  size_t n = x.size();

  // first compute the corresponding permutation that sorts x (required later)
  auto perm = tools::get_order(x);
  // actually sort x
  Eigen::VectorXd srt(n);
  for (size_t i = 0; i < n; ++i)
    srt(i) = x(perm(i));

  // compute contingency table
  Eigen::MatrixXd tab(n, 2);
  size_t lev = 0;
  size_t cnt = 1;
  for (size_t k = 1; k < n; ++k) {
    if (srt(k - 1) != srt(k)) {
      tab(lev, 0) = srt(k - 1);
      tab(lev++, 1) = cnt;
      cnt = 1;
    } else {
      cnt++;
      if (k == n - 1) {
        tab(lev, 0) = srt(k);
        tab(lev++, 1) = cnt;
      }
    }
  }
  tab.conservativeResize(lev, 2);

  // add deterministic, conditionally uniorm noise
  Eigen::VectorXd noise = Eigen::VectorXd::Zero(n);
  size_t i = 0;
  for (long k = 0; k < tab.rows(); ++k) {
    for (size_t cnt = 1; cnt <= tab(k, 1); ++cnt)
      noise(i++) = -0.5 + cnt / (tab(k, 1) + 1.0);
    cnt = 1;
  }
  Eigen::VectorXd jtr = srt + noise;

  // invert the permutation to return jittered x in original order
  for (long i = 0; i < perm.size(); ++i)
    srt(perm(i)) = jtr(i);

  return srt;
}

//! @brief simulates from the standard uniform distribution.
//!
//! @param n number of observations.
//! @param seeds seeds of the random number generator; if empty (default),
//!   the random number generator is seeded randomly.
//!
//! @return An size n vector of independent \f$ \mathrm{U}[0, 1] \f$ random
//!   variables.
inline Eigen::VectorXd
simulate_uniform(size_t n, std::vector<int> seeds)
{
  if (n < 1)
    throw std::runtime_error("n  must be at least 1.");

  if (seeds.size() == 0) { // no seeds provided, seed randomly
    std::random_device rd{};
    seeds = std::vector<int>(5);
    for (auto& s : seeds)
      s = static_cast<int>(rd());
  }

  // initialize random engine and uniform distribution
  std::seed_seq seq(seeds.begin(), seeds.end());
  std::mt19937 generator(seq);
  std::uniform_real_distribution<double> distribution(0.0, 1.0);

  Eigen::VectorXd U(n);
  return U.unaryExpr([&](double) { return distribution(generator); });
}

} // end kde1d::stats

} // end kde1d


#include "incidencematrices.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <array>
#include <memory>

namespace IncidenceMatrices {

/** @brief Create the mesh consisting of a triangle and quadrilateral
 *         from the exercise sheet.
 * @return Shared pointer to the hybrid2d mesh.
 */
std::shared_ptr<lf::mesh::Mesh> createDemoMesh() {
  // builder for a hybrid mesh in a world of dimension 2
  std::shared_ptr<lf::mesh::hybrid2d::MeshFactory> mesh_factory_ptr =
      std::make_shared<lf::mesh::hybrid2d::MeshFactory>(2);

  // Add points
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0, 0});    // (0)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{1, 0});    // (1)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{1, 1});    // (2)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0, 1});    // (3)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0.5, 1});  // (4)

  // Add the triangle
  // First set the coordinates of its nodes:
  Eigen::MatrixXd nodesOfTria(2, 3);
  nodesOfTria << 1, 1, 0.5, 0, 1, 1;
  mesh_factory_ptr->AddEntity(
      lf::base::RefEl::kTria(),  // we want a triangle
      std::array<lf::mesh::Mesh::size_type, 3>{
          {1, 2, 4}},  // indices of the nodes
      std::make_unique<lf::geometry::TriaO1>(nodesOfTria));  // node coords

  // Add the quadrilateral
  Eigen::MatrixXd nodesOfQuad(2, 4);
  nodesOfQuad << 0, 1, 0.5, 0, 0, 0, 1, 1;
  mesh_factory_ptr->AddEntity(
      lf::base::RefEl::kQuad(),
      std::array<lf::mesh::Mesh::size_type, 4>{{0, 1, 4, 3}},
      std::make_unique<lf::geometry::QuadO1>(nodesOfQuad));

  std::shared_ptr<lf::mesh::Mesh> demoMesh_p = mesh_factory_ptr->Build();

  return demoMesh_p;
}

/** @brief Compute the edge-vertex incidence matrix G for a given mesh
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *        such as lf::mesh::hybrid2d::Mesh)
 * @return The edge-vertex incidence matrix as Eigen::SparseMatrix<int>
 */
/* SAM_LISTING_BEGIN_1 */
Eigen::SparseMatrix<int> computeEdgeVertexIncidenceMatrix(
    const lf::mesh::Mesh &mesh) {
  // Store edge-vertex incidence matrix here

  // number of edges
  int N = mesh.NumEntities(1);
  // number of vertices
  int M = mesh.NumEntities(2);

  Eigen::SparseMatrix<int, Eigen::RowMajor> G(N,M);

  G.reserve(Eigen::VectorXi::Constant(N, 2));

  for(int i = 0; i < N; i++) {
    auto vertices = mesh.Entities(1)[i]->SubEntities(1);
    G.coeffRef(i, mesh.Index(*vertices[0])) = 1;
    G.coeffRef(i, mesh.Index(*vertices[1])) = -1;
  }

  return G;
}
/* SAM_LISTING_END_1 */

/** @brief Compute the cell-edge incidence matrix D for a given mesh
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *        such as lf::mesh::hybrid2d::Mesh)
 * @return The cell-edge incidence matrix as Eigen::SparseMatrix<int>
 */
/* SAM_LISTING_BEGIN_2 */
Eigen::SparseMatrix<int> computeCellEdgeIncidenceMatrix(
    const lf::mesh::Mesh &mesh) {

  // number of cells
  int N = mesh.NumEntities(0);
  // number of edges
  int M = mesh.NumEntities(1);

  Eigen::SparseMatrix<int, Eigen::RowMajor> D(N,M);

  D.reserve(Eigen::VectorXi::Constant(N, 4));

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

    auto edges = mesh.Entities(0)[i]->SubEntities(1);

    for(int k = 0; k < edges.size(); k++) {
      D.coeffRef(i, mesh.Index(*edges[k])) = lf::mesh::to_sign(mesh.Entities(0)[i]->RelativeOrientations()[k]);
    }

  }

  return D;
}
/* SAM_LISTING_END_2 */

/** @brief For a given mesh test if the product of cell-edge and edge-vertex
 *        incidence matrix is zero: D*G == 0?
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *             such as lf::mesh::hybrid2d::Mesh)
 * @return true, if the product is zero and false otherwise
 */
/* SAM_LISTING_BEGIN_3 */
bool testZeroIncidenceMatrixProduct(const lf::mesh::Mesh &mesh) {
  return (computeCellEdgeIncidenceMatrix(mesh) * computeEdgeVertexIncidenceMatrix(mesh)).norm() == 0;
}
/* SAM_LISTING_END_3 */

}  // namespace IncidenceMatrices


/*
(c) 2019 M. Werner - Part of the GIS++ tutorial
- https://www.martinwerner.de/teaching/spatial-cpp
- https://github.com/mwernerds/spatial-cpp

Program: Points
Compile: g++ -I $(BOOST_DIR) -Wall -std=c++11  -o 02_simplefeatures 02_simplefeatures.cpp
*/

#include<iostream>
#include<fstream>
#include <boost/geometry.hpp>

namespace bg = boost::geometry;

typedef bg::model::point<double, 2, bg::cs::cartesian> point;
typedef bg::model::box<point> box;
typedef bg::model::linestring<point> linestring;
typedef bg::model::polygon<point, false, false> polygon; // ccw, open polygon

// Note: the higher order objects usually have all features of a random_access container (or actually are some)

int main(int argc, char **argv)
{
    // let us make a nice triangle
    point a(0,0),b(1,1),c(2,0);
    polygon triangle;
    bg::append(bg::exterior_ring(triangle),a);
    bg::append(bg::exterior_ring(triangle),b);
    bg::append(bg::exterior_ring(triangle),c);
    std::cout << bg::wkt(a) << std::endl;
    std::cout << bg::wkt(b) << std::endl;
    std::cout << bg::wkt(c) << std::endl;
    std::cout << bg::wkt(triangle) << std::endl;

    // point in polygon (within relation)
    std::cout << std::boolalpha; // we want all bools to be written as true/false

    bg::correct(triangle); // make it CCW!
    std::cout << "Corrected Geometry: " << bg::wkt(triangle) << std::endl;
    std::cout << "Within: " << bg::within (bg::make<point>(1.0,0.5),triangle) << std::endl;

    // Let us now create our first file for QGIS:
    //
    // Algorithm in short:
    // 1) take two random (integer) triangles in [0,0]-[10,10].
    // 2) compute most important relations
    // 3) write a CSV file using WKT containing the two polygons for inspection

    std::srand(std::time(0)); //use current time as seed for random generator

    auto random_point = []()->point {return bg::make<point>(static_cast<int> (std::rand()%10),static_cast<int> (std::rand()%10));};

    //std::cout << bg::wkt(random_point()) << std::endl;
    polygon A,B;
    for (size_t i=0; i<3; i++) bg::append(bg::exterior_ring(A),random_point());
    for (size_t i=0; i<3; i++) bg::append(bg::exterior_ring(B),random_point());

    // remove these lines to see catastrophic results including negative area!
    bg::correct(A);
    bg::correct(B);

    std::cout << bg::wkt(A) << std::endl;
    std::cout << bg::wkt(B) << std::endl;

    // DE-9IM Matrix
    bg::de9im::matrix matrix = boost::geometry::relation(A,B);
    std::string code = matrix.str();
    std::cout << "relation: " << code << std::endl;
    // generic relate operation:

    bg::de9im::mask mask("T*F**F***"); // within
    auto p = random_point();
    bool check = bg::relate(p, A, mask);
    std::cout << "A random point " << bg::wkt(p) << " related: " << check << std::endl;

    // some algorithms relations:

    std::cout << "area: " << bg::area(A) << std::endl;
    std::cout << "covered_by: " <<  bg::covered_by (A,B) << std::endl;
    std::cout << "disjoint: " <<  bg::disjoint(A,B) << std::endl;
    std::cout << "equals: " <<  bg::equals(A,B) << std::endl;
    std::cout << "intersects: " <<  bg::intersects   (A,B) << std::endl;
    std::cout << "overlaps: " <<  bg::overlaps (A,B) << std::endl;
    std::cout << "touches: " <<  bg::touches (A,B) << std::endl;
    std::cout << "within: " <<  bg::within (A,B) << std::endl;

    // write CSV
    {
    std::ofstream ofs("geometry.csv");
    ofs << "wkt" << std::endl;
    ofs << bg::wkt(A)<< std::endl;
    ofs << bg::wkt(B) << std::endl;
    ofs.close();
    }

    // and now let us test within with random floating points
    {
    std::ofstream ofs("points.csv");
    ofs << "wkt; within" << std::endl;
    for (size_t i=0; i < 500; i++)
    {
	auto p = bg::make<point>( static_cast<double>(std::rand())/RAND_MAX*10.0,static_cast<double>(std::rand())/RAND_MAX*10.0);
	auto rel1 = bg::within(p,A);
	auto rel2 = bg::within(p,B);
	int score = (rel1 << 1) + rel2;
	ofs << bg::wkt(p) << ";" << score << std::endl;
    }
    }

    return 0;
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2009 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Authors: Toby D. Young, Polish Academy of Sciences,
 *          Wolfgang Bangerth, Texas A&M University
 */

// @sect3{Include files}

// 正如介绍中提到的，本程序基本上只是  step-4  的一个小修改版本。因此，以下大部分的include文件都是在那里使用的，或者至少是在以前的教程程序中已经使用的。

#include <deal.II/base/logstream.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/function_parser.h>
#include <deal.II/base/parameter_handler.h>
#include <deal.II/base/utilities.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/full_matrix.h>

// IndexSet用于设置每个  PETScWrappers::MPI::Vector:  的大小。
#include <deal.II/base/index_set.h>

// PETSc出现在这里是因为SLEPc依赖于这个库。

#include <deal.II/lac/petsc_sparse_matrix.h>
#include <deal.II/lac/petsc_vector.h>

// 然后我们需要实际导入SLEPc提供的求解器接口。

#include <deal.II/lac/slepc_solver.h>

// 我们还需要一些标准的C++。

#include <fstream>
#include <iostream>

// 最后，和以前的程序一样，我们将所有的deal.II类和函数名导入到本程序中所有的名字空间中。

namespace Step36
{
  using namespace dealii;
// @sect3{The EigenvalueProblem class template}

// 下面是主类模板的类声明。它看起来和在  step-4  中已经展示过的差不多了。

  template <int dim>
  class EigenvalueProblem
  {
  public:
    EigenvalueProblem(const std::string &prm_file);
    void run();

  private:
    void         make_grid_and_dofs();
    void         assemble_system();
    unsigned int solve();
    void         output_results() const;

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

// 有了这些例外情况。对于我们的特征值问题，我们既需要左手边的刚度矩阵，也需要右手边的质量矩阵。我们还需要的不仅仅是一个解函数，而是一整套我们想要计算的特征函数，以及相应的特征值。

    PETScWrappers::SparseMatrix             stiffness_matrix, mass_matrix;
    std::vector<PETScWrappers::MPI::Vector> eigenfunctions;
    std::vector<double>                     eigenvalues;

// 然后，我们需要一个对象来存储几个运行时参数，我们将在输入文件中指定。

    ParameterHandler parameters;

// 最后，我们将有一个对象，包含对我们自由度的 "约束"。如果我们有自适应细化的网格（目前的程序中没有），这可能包括悬挂节点约束。这里，我们将存储边界节点的约束  $U_i=0$  。

    AffineConstraints<double> constraints;
  };
// @sect3{Implementation of the EigenvalueProblem class}
// @sect4{EigenvalueProblem::EigenvalueProblem}

// 首先是构造函数。主要的新部分是处理运行时的输入参数。我们需要首先声明它们的存在，然后从输入文件中读取它们的值，该文件的名称被指定为该函数的参数。

  template <int dim>
  EigenvalueProblem<dim>::EigenvalueProblem(const std::string &prm_file)
    : fe(1)
    , dof_handler(triangulation)
  {

// TODO研究为什么获得正确的特征值退化所需的最小细化步骤数为6

    parameters.declare_entry(
      "Global mesh refinement steps",
      "5",
      Patterns::Integer(0, 20),
      "The number of times the 1-cell coarse mesh should "
      "be refined globally for our computations.");
    parameters.declare_entry("Number of eigenvalues/eigenfunctions",
                             "5",
                             Patterns::Integer(0, 100),
                             "The number of eigenvalues/eigenfunctions "
                             "to be computed.");
    parameters.declare_entry("Potential",
                             "0",
                             Patterns::Anything(),
                             "A functional description of the potential.");

    parameters.parse_input(prm_file);
  }
// @sect4{EigenvalueProblem::make_grid_and_dofs}

// 下一个函数在域 $[-1,1]^d$ 上创建一个网格，根据输入文件的要求对其进行多次细化，然后给它附加一个DoFHandler，将矩阵和向量初始化为正确的大小。我们还建立了对应于边界值的约束  $u|_{\partial\Omega}=0$  。

// 对于矩阵，我们使用PETSc包装器。这些包装器能够在非零条目被添加时分配必要的内存。这看起来效率很低：我们可以先计算稀疏模式，用它来初始化矩阵，然后在我们插入条目时，我们可以确定我们不需要重新分配内存和释放之前使用的内存。一种方法是使用这样的代码。用
// @code
//    DynamicSparsityPattern
//       dsp (dof_handler.n_dofs(),
//            dof_handler.n_dofs());
//    DoFTools::make_sparsity_pattern (dof_handler, dsp);
//    dsp.compress ();
//    stiffness_matrix.reinit (dsp);
//    mass_matrix.reinit (dsp);
//  @endcode
//  代替下面两个 reinit() 的刚度和质量矩阵的调用。

// 不幸的是，这并不完全可行。上面的代码可能会导致在非零模式下的一些条目，我们只写零条目；最值得注意的是，对于那些属于边界节点的行和列的非对角线条目，这一点是成立的。这不应该是一个问题，但是不管什么原因，PETSc的ILU预处理程序（我们用来解决特征值求解器中的线性系统）不喜欢这些额外的条目，并以错误信息中止。

// 在没有任何明显的方法来避免这种情况的情况下，我们干脆选择第二种最好的方法，即让PETSc在必要时分配内存。也就是说，由于这不是一个时间上的关键部分，这整个事件就不再重要了。

  template <int dim>
  void EigenvalueProblem<dim>::make_grid_and_dofs()
  {
    GridGenerator::hyper_cube(triangulation, -1, 1);
    triangulation.refine_global(
      parameters.get_integer("Global mesh refinement steps"));
    dof_handler.distribute_dofs(fe);

    DoFTools::make_zero_boundary_constraints(dof_handler, constraints);
    constraints.close();

    stiffness_matrix.reinit(dof_handler.n_dofs(),
                            dof_handler.n_dofs(),
                            dof_handler.max_couplings_between_dofs());
    mass_matrix.reinit(dof_handler.n_dofs(),
                       dof_handler.n_dofs(),
                       dof_handler.max_couplings_between_dofs());

// 下一步是处理特征谱的问题。在这种情况下，输出是特征值和特征函数，所以我们将特征函数和特征值列表的大小设置为与我们在输入文件中要求的一样大。当使用 PETScWrappers::MPI::Vector, 时，Vector是使用IndexSet初始化的。IndexSet不仅用于调整 PETScWrappers::MPI::Vector 的大小，而且还将 PETScWrappers::MPI::Vector 中的一个索引与一个自由度联系起来（更详细的解释见 step-40 ）。函数complete_index_set()创建了一个IndexSet，每个有效的索引都是这个集合的一部分。请注意，这个程序只能按顺序运行，如果并行使用，将抛出一个异常。

    IndexSet eigenfunction_index_set = dof_handler.locally_owned_dofs();
    eigenfunctions.resize(
      parameters.get_integer("Number of eigenvalues/eigenfunctions"));
    for (unsigned int i = 0; i < eigenfunctions.size(); ++i)
      eigenfunctions[i].reinit(eigenfunction_index_set, MPI_COMM_WORLD);

    eigenvalues.resize(eigenfunctions.size());
  }
// @sect4{EigenvalueProblem::assemble_system}

// 在这里，我们从局部贡献 $A^K_{ij} = \int_K \nabla\varphi_i(\mathbf x) \cdot \nabla\varphi_j(\mathbf x) + V(\mathbf x)\varphi_i(\mathbf x)\varphi_j(\mathbf x)$ 和 $M^K_{ij} = \int_K \varphi_i(\mathbf x)\varphi_j(\mathbf x)$ 中分别组合出全局刚度和质量矩阵。如果你看过以前的教程程序，这个函数应该会很熟悉。唯一新的东西是使用我们从输入文件中得到的表达式，设置一个描述势 $V(\mathbf x)$ 的对象。然后我们需要在每个单元的正交点上评估这个对象。如果你见过如何评估函数对象（例如，见 step-5 中的系数），这里的代码也会显得相当熟悉。

  template <int dim>
  void EigenvalueProblem<dim>::assemble_system()
  {
    QGauss<dim> quadrature_formula(fe.degree + 1);

    FEValues<dim> fe_values(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_stiffness_matrix(dofs_per_cell, dofs_per_cell);
    FullMatrix<double> cell_mass_matrix(dofs_per_cell, dofs_per_cell);

    std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

    FunctionParser<dim> potential;
    potential.initialize(FunctionParser<dim>::default_variable_names(),
                         parameters.get("Potential"),
                         typename FunctionParser<dim>::ConstMap());

    std::vector<double> potential_values(n_q_points);
    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        fe_values.reinit(cell);
        cell_stiffness_matrix = 0;
        cell_mass_matrix      = 0;

        potential.value_list(fe_values.get_quadrature_points(),
                             potential_values);

        for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
          for (unsigned int i = 0; i < dofs_per_cell; ++i)
            for (unsigned int j = 0; j < dofs_per_cell; ++j)
              {
                cell_stiffness_matrix(i, j) +=           //
                  (fe_values.shape_grad(i, q_point) *    //
                     fe_values.shape_grad(j, q_point)    //
                   +                                     //
                   potential_values[q_point] *           //
                     fe_values.shape_value(i, q_point) * //
                     fe_values.shape_value(j, q_point)   //
                   ) *                                   //
                  fe_values.JxW(q_point);                //

                cell_mass_matrix(i, j) +=              //
                  (fe_values.shape_value(i, q_point) * //
                   fe_values.shape_value(j, q_point)   //
                   ) *                                 //
                  fe_values.JxW(q_point);              //
              }

// 现在我们有了本地矩阵的贡献，我们把它们转移到全局对象中，并处理好零边界约束。

        cell->get_dof_indices(local_dof_indices);

        constraints.distribute_local_to_global(cell_stiffness_matrix,
                                               local_dof_indices,
                                               stiffness_matrix);
        constraints.distribute_local_to_global(cell_mass_matrix,
                                               local_dof_indices,
                                               mass_matrix);
      }

// 在函数的最后，我们告诉PETSc，矩阵现在已经完全组装好了，稀疏矩阵表示法现在可以被压缩了，因为不会再添加任何条目。

    stiffness_matrix.compress(VectorOperation::add);
    mass_matrix.compress(VectorOperation::add);

// 在离开函数之前，我们计算虚假的特征值，这些特征值是由零Dirichlet约束引入到系统中的。正如介绍中所讨论的，使用Dirichlet边界条件，加上位于域的边界的自由度仍然是我们所求解的线性系统的一部分，引入了一些虚假的特征值。下面，我们输出它们所处的区间，以确保我们在计算中出现时可以忽略它们。

    double min_spurious_eigenvalue = std::numeric_limits<double>::max(),
           max_spurious_eigenvalue = -std::numeric_limits<double>::max();

    for (unsigned int i = 0; i < dof_handler.n_dofs(); ++i)
      if (constraints.is_constrained(i))
        {
          const double ev         = stiffness_matrix(i, i) / mass_matrix(i, i);
          min_spurious_eigenvalue = std::min(min_spurious_eigenvalue, ev);
          max_spurious_eigenvalue = std::max(max_spurious_eigenvalue, ev);
        }

    std::cout << "   Spurious eigenvalues are all in the interval "
              << "[" << min_spurious_eigenvalue << ","
              << max_spurious_eigenvalue << "]" << std::endl;
  }
// @sect4{EigenvalueProblem::solve}

// 这是该程序的关键新功能。现在系统已经设置好了，现在是实际解决问题的好时机：和其他例子一样，这是使用 "解决 "程序来完成的。从本质上讲，它的工作原理与其他程序一样：你设置一个SolverControl对象，描述我们要解决的线性系统的精度，然后我们选择我们想要的解算器类型。这里我们选择了SLEPc的Krylov-Schur求解器，对于这类问题来说，这是一个相当快速和强大的选择。

  template <int dim>
  unsigned int EigenvalueProblem<dim>::solve()
  {

// 我们从这里开始，就像我们通常做的那样，指定我们想要的收敛控制。

    SolverControl                    solver_control(dof_handler.n_dofs(), 1e-9);
    SLEPcWrappers::SolverKrylovSchur eigensolver(solver_control);

// 在我们实际求解特征函数和-值之前，我们还必须选择哪一组特征值来求解。让我们选择那些实部最小的特征值和相应的特征函数（事实上，我们在这里解决的问题是对称的，所以特征值是纯实部的）。之后，我们就可以真正让SLEPc做它的工作了。

    eigensolver.set_which_eigenpairs(EPS_SMALLEST_REAL);

    eigensolver.set_problem_type(EPS_GHEP);

    eigensolver.solve(stiffness_matrix,
                      mass_matrix,
                      eigenvalues,
                      eigenfunctions,
                      eigenfunctions.size());

// 上述调用的输出是一组向量和数值。在特征值问题中，特征函数只确定到一个常数，这个常数可以很随意地固定。由于对特征值问题的原点一无所知，SLEPc除了将特征向量归一到 $l_2$ （向量）准则外，没有其他选择。不幸的是，这个规范与我们从特征函数角度可能感兴趣的任何规范没有什么关系： $L_2(\Omega)$ 规范，或者也许是 $L_\infty(\Omega)$ 规范。

//让我们选择后者，重新划分特征函数的尺度，使其具有 $\|\phi_i(\mathbf x)\|_{L^\infty(\Omega)}=1$ 而不是 $\|\Phi\|_{l_2}=1$ （其中 $\phi_i$ 是 $i$ 第三个特征function， $\Phi_i$ 是相应的结点值矢量）。对于这里选择的 $Q_1$ 元素，我们知道函数 $\phi_i(\mathbf x)$ 的最大值是在其中一个节点达到的，所以 $\max_{\mathbf x}\phi_i(\mathbf x)=\max_j (\Phi_i)_j$ ，使得在 $L_\infty$ 准则下的归一化是微不足道的。请注意，如果我们选择 $Q_k$ 元素与 $k>1$ ，这就不容易了：在那里，一个函数的最大值不一定要在一个节点上达到，所以 $\max_{\mathbf x}\phi_i(\mathbf x)\ge\max_j (\Phi_i)_j$ （尽管平等通常几乎是真的）。

    for (unsigned int i = 0; i < eigenfunctions.size(); ++i)
      eigenfunctions[i] /= eigenfunctions[i].linfty_norm();

// 最后返回收敛所需的迭代次数。

    return solver_control.last_step();
  }
// @sect4{EigenvalueProblem::output_results}

// 这是本程序的最后一个重要功能。它使用DataOut类来生成特征函数的图形输出，以便以后进行可视化。它的工作原理与其他许多教程中的程序一样。

// 整个函数的集合被输出为一个单一的VTK文件。

  template <int dim>
  void EigenvalueProblem<dim>::output_results() const
  {
    DataOut<dim> data_out;

    data_out.attach_dof_handler(dof_handler);

    for (unsigned int i = 0; i < eigenfunctions.size(); ++i)
      data_out.add_data_vector(eigenfunctions[i],
                               std::string("eigenfunction_") +
                                 Utilities::int_to_string(i));

// 唯一值得讨论的可能是，由于势在输入文件中被指定为函数表达式，因此最好能将其与特征函数一起以图形形式表示。实现这一目的的过程相对简单：我们建立一个代表 $V(\mathbf x)$ 的对象，然后将这个连续函数插值到有限元空间。我们还将结果附加到DataOut对象上，以便进行可视化。

    Vector<double> projected_potential(dof_handler.n_dofs());
    {
      FunctionParser<dim> potential;
      potential.initialize(FunctionParser<dim>::default_variable_names(),
                           parameters.get("Potential"),
                           typename FunctionParser<dim>::ConstMap());
      VectorTools::interpolate(dof_handler, potential, projected_potential);
    }
    data_out.add_data_vector(projected_potential, "interpolated_potential");

    data_out.build_patches();

    std::ofstream output("eigenvectors.vtk");
    data_out.write_vtk(output);
  }
// @sect4{EigenvalueProblem::run}

// 这是一个对一切都有顶层控制的函数。它几乎与  step-4  中的内容完全相同。

  template <int dim>
  void EigenvalueProblem<dim>::run()
  {
    make_grid_and_dofs();

    std::cout << "   Number of active cells:       "
              << triangulation.n_active_cells() << std::endl
              << "   Number of degrees of freedom: " << dof_handler.n_dofs()
              << std::endl;

    assemble_system();

    const unsigned int n_iterations = solve();
    std::cout << "   Solver converged in " << n_iterations << " iterations."
              << std::endl;

    output_results();

    std::cout << std::endl;
    for (unsigned int i = 0; i < eigenvalues.size(); ++i)
      std::cout << "      Eigenvalue " << i << " : " << eigenvalues[i]
                << std::endl;
  }
} // namespace Step36
// @sect3{The main function}
int main(int argc, char **argv)
{
  try
    {
      using namespace dealii;
      using namespace Step36;

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

// 这个程序只能在串行中运行。否则，将抛出一个异常。

      AssertThrow(Utilities::MPI::n_mpi_processes(MPI_COMM_WORLD) == 1,
                  ExcMessage(
                    "This program can only be run in serial, use ./step-36"));

      EigenvalueProblem<2> problem("step-36.prm");
      problem.run();
    }

// 在这期间，我们一直在注意是否有任何异常应该被生成。如果是这样的话，我们就会惊慌失措...

  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

// 如果没有抛出异常，我们就告诉程序不要再胡闹了，乖乖地退出。

  std::cout << std::endl << "   Job done." << std::endl;

  return 0;
}


#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <cmath>
#include <Eigen/StdVector>
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>

using namespace std;

// path to trajectory file
string estimated_file = "./estimated.txt";
string groundtruth_file = "./groundtruth.txt";

void DrawTrajectory(vector<Sophus::SE3> estimated_poses,vector<Sophus::SE3> groundtruth_poses) {
    if (estimated_poses.empty() || groundtruth_poses.empty()) {
        cerr << "Trajectory is empty!" << endl;
        return;
    }

    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glLineWidth(2);

        for (size_t i = 0; i < estimated_poses.size() - 1; i++) {
            glColor3f(1 - (float) i / estimated_poses.size(), 0.0f, (float) i / estimated_poses.size());
            glBegin(GL_LINES);
            auto p1 = estimated_poses[i], p2 = estimated_poses[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }

        for (size_t i = 0; i < groundtruth_poses.size() - 1; i++) {
            glColor3f(1 - (float) i / groundtruth_poses.size(), 0.0f, (float) i / groundtruth_poses.size());
            glBegin(GL_LINES);
            auto p1 = groundtruth_poses[i], p2 = groundtruth_poses[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }

        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }

}

vector<Sophus::SE3> readPose(string filename)
{
    vector<Sophus::SE3> poses;
    /// implement pose reading code
    // start your code here (5~10 lines)

    ifstream fin(filename);
    for ( int i=0; i<612; i++ )
    {
        double data[8] = {0};
        for ( auto& d:data )
            fin>>d;
        Eigen::Quaterniond q( data[7], data[4], data[5], data[6] );
        Eigen::Vector3d t(data[1], data[2], data[3]);
        Sophus::SE3 SE3_qt(q,t);
        poses.push_back(SE3_qt);
    }

    return poses;
    // end your code here

}

double RMSE(vector<Sophus::SE3> estimated_poses,vector<Sophus::SE3> groundtruth_poses)
{
    double sum = 0;
    for(int i=0;i<estimated_poses.size();i++)
    {
        double error = 0;
        double e = sqrt(  (groundtruth_poses.at(i).inverse() * estimated_poses.at(i)).log().transpose() *   (groundtruth_poses.at(i).inverse() * estimated_poses.at(i)).log()   );

        error = pow(abs(e),2);
        sum+= error;
    }
    double rmse = pow(sum/estimated_poses.size(),0.5);
    return rmse;
}

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

    vector<Sophus::SE3> estimated_poses;
    vector<Sophus::SE3> groundtruth_poses;

    estimated_poses = readPose(estimated_file);
    groundtruth_poses = readPose(groundtruth_file);

    std::cout<<RMSE(estimated_poses,groundtruth_poses)<<endl;;
    DrawTrajectory(estimated_poses,groundtruth_poses);
    return 0;
}