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

#ifndef OPENGV_RELATIVE_POSE_MODULES_MAIN_HPP_
#define OPENGV_RELATIVE_POSE_MODULES_MAIN_HPP_

#include <stdlib.h>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
#include <opengv/types.hpp>

namespace opengv
{
namespace relative_pose
{
namespace modules
{
rotation_t sixpt_urban_main_onlyRot(
		const bearingVectors_t& f1,
		const bearingVectors_t& f2);
transformation_t sixpt_urban_main(
	const bearingVectors_t& f1,
	const bearingVectors_t& f2);
void fivept_stewenius_main(
    const Eigen::Matrix<double,9,4> & EE,
    complexEssentials_t & complexEssentials );
void fivept_nister_main(
    const Eigen::Matrix<double,9,4> & EE,
    essentials_t & essentials );
void fivept_kneip_main(
    const Eigen::Matrix<double,3,5> & f1,
    const Eigen::Matrix<double,3,5> & f2,
    rotations_t & rotations );
void eigensolver_main(
    const Eigen::Matrix3d & xxF,
    const Eigen::Matrix3d & yyF,
    const Eigen::Matrix3d & zzF,
    const Eigen::Matrix3d & xyF,
    const Eigen::Matrix3d & yzF,
    const Eigen::Matrix3d & zxF,
    eigensolverOutput_t & output );
void sixpt_main(
    Eigen::Matrix<double,6,6> & L1,
    Eigen::Matrix<double,6,6> & L2,
    rotations_t & solutions);
void sixpt_ventura_main(
	const Eigen::Matrix<double, 6, 6> &w1,
	const Eigen::Matrix<double, 6, 6> &w2,
	const Eigen::Matrix<double, 6, 6> &w3,
	const Eigen::Matrix<double, 6, 6> &w4,
	const Eigen::Matrix<double, 6, 6> &w5,
	const Eigen::Matrix<double, 6, 6> &w6,
	std::vector<Eigen::Vector3d> &rsolns);
void ge_main(
    const Eigen::Matrix3d & xxF,
    const Eigen::Matrix3d & yyF,
    const Eigen::Matrix3d & zzF,
    const Eigen::Matrix3d & xyF,
    const Eigen::Matrix3d & yzF,
    const Eigen::Matrix3d & zxF,
    const Eigen::Matrix<double,3,9> & x1P,
    const Eigen::Matrix<double,3,9> & y1P,
    const Eigen::Matrix<double,3,9> & z1P,
    const Eigen::Matrix<double,3,9> & x2P,
    const Eigen::Matrix<double,3,9> & y2P,
    const Eigen::Matrix<double,3,9> & z2P,
    const Eigen::Matrix<double,9,9> & m11P,
    const Eigen::Matrix<double,9,9> & m12P,
    const Eigen::Matrix<double,9,9> & m22P,
    const cayley_t & startingPoint,
    geOutput_t & output );
void ge_main2(
    const Eigen::Matrix3d & xxF,
    const Eigen::Matrix3d & yyF,
    const Eigen::Matrix3d & zzF,
    const Eigen::Matrix3d & xyF,
    const Eigen::Matrix3d & yzF,
    const Eigen::Matrix3d & zxF,
    const Eigen::Matrix<double,3,9> & x1P,
    const Eigen::Matrix<double,3,9> & y1P,
    const Eigen::Matrix<double,3,9> & z1P,
    const Eigen::Matrix<double,3,9> & x2P,
    const Eigen::Matrix<double,3,9> & y2P,
    const Eigen::Matrix<double,3,9> & z2P,
    const Eigen::Matrix<double,9,9> & m11P,
    const Eigen::Matrix<double,9,9> & m12P,
    const Eigen::Matrix<double,9,9> & m22P,
    const cayley_t & startingPoint,
    geOutput_t & output );
void ge_plot(
    const Eigen::Matrix3d & xxF,
    const Eigen::Matrix3d & yyF,
    const Eigen::Matrix3d & zzF,
    const Eigen::Matrix3d & xyF,
    const Eigen::Matrix3d & yzF,
    const Eigen::Matrix3d & zxF,
    const Eigen::Matrix<double,3,9> & x1P,
    const Eigen::Matrix<double,3,9> & y1P,
    const Eigen::Matrix<double,3,9> & z1P,
    const Eigen::Matrix<double,3,9> & x2P,
    const Eigen::Matrix<double,3,9> & y2P,
    const Eigen::Matrix<double,3,9> & z2P,
    const Eigen::Matrix<double,9,9> & m11P,
    const Eigen::Matrix<double,9,9> & m12P,
    const Eigen::Matrix<double,9,9> & m22P,
    geOutput_t & output );
}
}
}

#endif /* OPENGV_RELATIVE_POSE_MODULES_MAIN_HPP_ */


#include "storm/constants.hpp"
#include "storm/rambo.hpp"
#include "storm/simple.hpp"
#include "storm/types.hpp"
#include <array>
#include <boost/math/quadrature/gauss_kronrod.hpp>
#include <cmath>
#include <cstddef>
#include <functional>
#include <locale>
#include <stdexcept>
#include <utility>

namespace storm {

/**
 * Compute the partial width for a RH neutrino decaying into a active neutrino
 * and a Higgs.
 */
auto SimpleRhNeutrino::width_vl_h() const -> double {
  return ((-pow(kHIGGS_MASS, 2) + pow(p_mvr, 2)) * pow(p_theta, 2) *
          std::abs(pow(kHIGGS_MASS, 2) - pow(p_mvr, 2))) /
         (16. * p_mvr * M_PI * pow(kHIGGS_VEV, 2));
}

/**
 * Compute the partial width for a RH neutrino decaying into a active neutrino
 * and a Z.
 */
auto SimpleRhNeutrino::width_vl_z() const -> double {

  return (((pow(p_mvr, 4) + pow(p_mvr, 2) * pow(kZ_BOSON_MASS, 2) -
            2 * pow(kZ_BOSON_MASS, 4)) *
           kALPHA_EM * pow(p_theta, 2) *
           std::abs(pow(p_mvr, 2) - pow(kZ_BOSON_MASS, 2))) /
          (16.0 * pow(kCOS_THETA_WEAK, 2) * pow(p_mvr, 3) *
           pow(kZ_BOSON_MASS, 2) * pow(kSIN_THETA_WEAK, 2)));
}

/**
 * Compute the partial width for a RH neutrino decaying into a charged lepton
 * and a W.
 */
auto SimpleRhNeutrino::width_l_w() const -> double {
  return (std::sqrt(
              (p_ml - p_mvr - kW_BOSON_MASS) * (p_ml + p_mvr - kW_BOSON_MASS) *
              (p_ml - p_mvr + kW_BOSON_MASS) * (p_ml + p_mvr + kW_BOSON_MASS)) *
          (pow(pow(p_ml, 2) - pow(p_mvr, 2), 2) +
           (pow(p_ml, 2) + pow(p_mvr, 2)) * pow(kW_BOSON_MASS, 2) -
           2 * pow(kW_BOSON_MASS, 4)) *
          kALPHA_EM * pow(p_theta, 2)) /
         (16. * pow(p_mvr, 3) * pow(kW_BOSON_MASS, 2) *
          pow(kSIN_THETA_WEAK, 2));
}

/**
 * Compute the integration bounds on the Mandelstam variable `s` for three-body
 * phase-space integration.
 * @param m Mass of the decaying particle.
 * @param m1 Mass of final state particle 1.
 * @param m2 Mass of final state particle 2.
 * @param m3 Mass of final state particle 3.
 */
inline static auto compute_s_bounds(double m, double m1, double m2, double m3)
    -> std::pair<double, double> {
  return std::make_pair(pow(m2 + m3, 2), pow(m - m1, 2));
}

/**
 * @breif Compute the three body decay width given the squared matrix element
 * integrated over the Mandelstam variable `t`.
 * @param msqrd Squared matrix element integrated over t = (p1+p3)^2.
 * @param m Mass of the decaying particle.
 * @param fsp_masses Mass of the three final-state particles.
 * @param error Estimated error in the decay width.
 */
static auto compute_width_3body(const std::function<double(double)> &msqrd,
                                double m, std::array<double, 3> fsp_masses,
                                double *error = nullptr) -> double {
  using boost::math::quadrature::gauss_kronrod;

  auto bounds =
      compute_s_bounds(m, fsp_masses[0], fsp_masses[1], fsp_masses[2]);
  auto integral = gauss_kronrod<double, 15>::integrate(
      msqrd, bounds.first, bounds.second, 5, 1e-9, error);

  const double pf = 1.0 / (256.0 * pow(M_PI * m, 3));
  *error *= pf;
  return std::abs(integral) * pf;
}

static auto compute_width_3body(const SquaredMatrixElement &msqrd,
                                const double m, std::vector<double> fsp_masses,
                                size_t num_events = 10000,
                                double *error = nullptr) {

  Rambo rambo{std::move(fsp_masses), m, msqrd};
  const auto width = rambo.compute_width(num_events);
  *error = width.second;
  return width.first;
}

} // namespace storm


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2016-2021 Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fisikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is 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_GEOMETRY_STRATEGIES_CARTESIAN_AZIMUTH_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_AZIMUTH_HPP

#include <cmath>

#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/core/coordinate_promotion.hpp>

#include <boost/geometry/strategies/azimuth.hpp>

#include <boost/geometry/util/select_most_precise.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace azimuth
{

template <typename CalculationType = void>
class cartesian
{
public:
    template <typename T1, typename T2>
    struct result_type
        : geometry::select_most_precise
              <
                  // NOTE: this promotes any integer type to double
                  typename geometry::promote_floating_point<T1, double>::type,
                  typename geometry::promote_floating_point<T2, double>::type,
                  CalculationType
              >
    {};

    template <typename T1, typename T2, typename Result>
    static inline void apply(T1 const& x1, T1 const& y1,
                             T2 const& x2, T2 const& y2,
                             Result& a1, Result& a2)
    {
        compute(x1, y1, x2, y2, a1, a2);
    }
    template <typename T1, typename T2, typename Result>
    static inline void apply(T1 const& x1, T1 const& y1,
                             T2 const& x2, T2 const& y2,
                             Result& a1)
    {
        compute(x1, y1, x2, y2, a1, a1);
    }
    template <typename T1, typename T2, typename Result>
    static inline void apply_reverse(T1 const& x1, T1 const& y1,
                                     T2 const& x2, T2 const& y2,
                                     Result& a2)
    {
        compute(x1, y1, x2, y2, a2, a2);
    }

private:
    template <typename T1, typename T2, typename Result>
    static inline void compute(T1 const& x1, T1 const& y1,
                               T2 const& x2, T2 const& y2,
                               Result& a1, Result& a2)
    {
        typedef typename result_type<T1, T2>::type calc_t;

        // NOTE: azimuth 0 is at Y axis, increasing right
        // as in spherical/geographic where 0 is at North axis
        a1 = a2 = atan2(calc_t(x2) - calc_t(x1), calc_t(y2) - calc_t(y1));
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

template <>
struct default_strategy<cartesian_tag>
{
    typedef strategy::azimuth::cartesian<> type;
};

}

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::azimuth

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_AZIMUTH_HPP


/*
 [auto_generated]
 libs/numeric/odeint/examples/heun.cpp

 [begin_description]
 Examplary implementation of the method of Heun.
 [end_description]

 Copyright 2012 Karsten Ahnert
 Copyright 2012 Mario Mulansky

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <iostream>

#include <boost/fusion/container/vector.hpp>
#include <boost/fusion/container/generation/make_vector.hpp>

#include <boost/array.hpp>

#include <boost/numeric/odeint.hpp>

namespace fusion = boost::fusion;

//[ heun_define_coefficients
template< class Value = double >
struct heun_a1 : boost::array< Value , 1 > {
    heun_a1( void )
    {
        (*this)[0] = static_cast< Value >( 1 ) / static_cast< Value >( 3 );
    }
};

template< class Value = double >
struct heun_a2 : boost::array< Value , 2 >
{
    heun_a2( void )
    {
        (*this)[0] = static_cast< Value >( 0 );
        (*this)[1] = static_cast< Value >( 2 ) / static_cast< Value >( 3 );
    }
};

template< class Value = double >
struct heun_b : boost::array< Value , 3 >
{
    heun_b( void )
    {
        (*this)[0] = static_cast<Value>( 1 ) / static_cast<Value>( 4 );
        (*this)[1] = static_cast<Value>( 0 );
        (*this)[2] = static_cast<Value>( 3 ) / static_cast<Value>( 4 );
    }
};

template< class Value = double >
struct heun_c : boost::array< Value , 3 >
{
    heun_c( void )
    {
        (*this)[0] = static_cast< Value >( 0 );
        (*this)[1] = static_cast< Value >( 1 ) / static_cast< Value >( 3 );
        (*this)[2] = static_cast< Value >( 2 ) / static_cast< Value >( 3 );
    }
};
//]

//[ heun_stepper_definition
template<
    class State ,
    class Value = double ,
    class Deriv = State ,
    class Time = Value ,
    class Algebra = boost::numeric::odeint::range_algebra ,
    class Operations = boost::numeric::odeint::default_operations ,
    class Resizer = boost::numeric::odeint::initially_resizer
>
class heun : public
boost::numeric::odeint::explicit_generic_rk< 3 , 3 , State , Value , Deriv , Time ,
                                             Algebra , Operations , Resizer >
{

public:

    typedef boost::numeric::odeint::explicit_generic_rk< 3 , 3 , State , Value , Deriv , Time ,
                                                         Algebra , Operations , Resizer > stepper_base_type;

    typedef typename stepper_base_type::state_type state_type;
    typedef typename stepper_base_type::wrapped_state_type wrapped_state_type;
    typedef typename stepper_base_type::value_type value_type;
    typedef typename stepper_base_type::deriv_type deriv_type;
    typedef typename stepper_base_type::wrapped_deriv_type wrapped_deriv_type;
    typedef typename stepper_base_type::time_type time_type;
    typedef typename stepper_base_type::algebra_type algebra_type;
    typedef typename stepper_base_type::operations_type operations_type;
    typedef typename stepper_base_type::resizer_type resizer_type;
    typedef typename stepper_base_type::stepper_type stepper_type;

    heun( const algebra_type &algebra = algebra_type() )
    : stepper_base_type(
            fusion::make_vector(
                heun_a1<Value>() ,
                heun_a2<Value>() ) ,
            heun_b<Value>() , heun_c<Value>() , algebra )
    { }
};
//]

const double sigma = 10.0;
const double R = 28.0;
const double b = 8.0 / 3.0;

struct lorenz
{
    template< class State , class Deriv >
    void operator()( const State &x_ , Deriv &dxdt_ , double t ) const
    {
        typename boost::range_iterator< const State >::type x = boost::begin( x_ );
        typename boost::range_iterator< Deriv >::type dxdt = boost::begin( dxdt_ );

        dxdt[0] = sigma * ( x[1] - x[0] );
        dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
        dxdt[2] = -b * x[2] + x[0] * x[1];
    }
};

struct streaming_observer
{
    std::ostream &m_out;
    streaming_observer( std::ostream &out ) : m_out( out ) { }
    template< typename State , typename Value >
    void operator()( const State &x , Value t ) const
    {
        m_out << t;
        for( size_t i=0 ; i<x.size() ; ++i ) m_out << "\t" << x[i];
        m_out << "\n";
    }
};

int main( int argc , char **argv )
{
    using namespace std;
    using namespace boost::numeric::odeint;

    //[ heun_example
    typedef boost::array< double , 3 > state_type;
    heun< state_type > h;
    state_type x = {{ 10.0 , 10.0 , 10.0 }};

    integrate_const( h , lorenz() , x , 0.0 , 100.0 , 0.01 ,
                     streaming_observer( std::cout ) );

    //]

    return 0;
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 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.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Wolfgang Bangerth, Colorado State University, 2021.
 * Based on step-15 by Sven Wetterauer, University of Heidelberg, 2012.
 */

// @sect3{Include files}

// 这个程序开始时和其他大多数程序一样，有众所周知的包含文件。与 step-15 程序相比，我们在这里所做的大部分工作都是从该程序中复制的，唯一不同的是包括头文件，我们从该文件中导入了SparseDirectUMFPACK类和KINSOL的实际接口。

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

#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/sparse_direct.h>

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

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

#include <deal.II/fe/fe_values.h>
#include <deal.II/fe/fe_q.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/numerics/error_estimator.h>
#include <deal.II/numerics/solution_transfer.h>

#include <deal.II/sundials/kinsol.h>

#include <fstream>
#include <iostream>

namespace Step77
{
  using namespace dealii;
// @sect3{The MinimalSurfaceProblem class template}

// 同样地，这个程序的主类基本上是  step-15  中的一个副本。然而，该类确实将雅各布（系统）矩阵（以及使用直接求解器对其进行因式分解）和残差的计算分成了不同的函数，原因已在介绍中列出。出于同样的原因，该类也有一个指向雅各布矩阵因式分解的指针，该指针在我们每次更新雅各布矩阵时被重置。

// （如果你想知道为什么程序对雅各布矩阵使用直接对象，而对因式分解使用指针。每次KINSOL要求更新雅各布矩阵时，我们可以简单地写`jacobian_matrix=0;`将其重置为一个空矩阵，然后我们可以再次填充。另一方面，SparseDirectUMFPACK类没有办法扔掉它的内容或用新的因式分解来替换它，所以我们使用一个指针。我们只是扔掉整个对象，并在我们有新的雅各布矩阵需要分解时创建一个新的对象。)

// 最后，该类有一个定时器变量，我们将用它来评估程序的不同部分需要多长时间，这样我们就可以评估KINSOL的不重建矩阵及其因式分解的倾向是否合理。我们将在下面的 "结果 "部分讨论这个问题。

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

  private:
    void setup_system(const bool initial_step);
    void solve(const Vector<double> &rhs,
               Vector<double> &      solution,
               const double          tolerance);
    void refine_mesh();
    void output_results(const unsigned int refinement_cycle);
    void set_boundary_values();
    void compute_and_factorize_jacobian(const Vector<double> &evaluation_point);
    void compute_residual(const Vector<double> &evaluation_point,
                          Vector<double> &      residual);

    Triangulation<dim> triangulation;

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

    AffineConstraints<double> hanging_node_constraints;

    SparsityPattern                      sparsity_pattern;
    SparseMatrix<double>                 jacobian_matrix;
    std::unique_ptr<SparseDirectUMFPACK> jacobian_matrix_factorization;

    Vector<double> current_solution;

    TimerOutput computing_timer;
  };

//  @sect3{Boundary condition}

// 实现边界值的类是对  step-15  的复制。

  template <int dim>
  class BoundaryValues : public Function<dim>
  {
  public:
    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override;
  };

  template <int dim>
  double BoundaryValues<dim>::value(const Point<dim> &p,
                                    const unsigned int /*component*/) const
  {
    return std::sin(2 * numbers::PI * (p[0] + p[1]));
  }
// @sect3{The MinimalSurfaceProblem class implementation}
// @sect4{Constructor and set up functions}

// 下面的几个函数也基本上是复制了 step-15 已经做的事情，所以没有什么可讨论的。

  template <int dim>
  MinimalSurfaceProblem<dim>::MinimalSurfaceProblem()
    : dof_handler(triangulation)
    , fe(1)
    , computing_timer(std::cout, TimerOutput::never, TimerOutput::wall_times)
  {}

  template <int dim>
  void MinimalSurfaceProblem<dim>::setup_system(const bool initial_step)
  {
    TimerOutput::Scope t(computing_timer, "set up");

    if (initial_step)
      {
        dof_handler.distribute_dofs(fe);
        current_solution.reinit(dof_handler.n_dofs());

        hanging_node_constraints.clear();
        DoFTools::make_hanging_node_constraints(dof_handler,
                                                hanging_node_constraints);
        hanging_node_constraints.close();
      }

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

    hanging_node_constraints.condense(dsp);

    sparsity_pattern.copy_from(dsp);
    jacobian_matrix.reinit(sparsity_pattern);
    jacobian_matrix_factorization.reset();
  }

//  @sect4{Assembling and factorizing the Jacobian matrix}

// 然后，下面的函数负责对雅各布矩阵进行组装和因子化。该函数的前半部分实质上是 step-15 的`assemble_system()`函数，只是它没有处理同时形成右手边的向量（即残差），因为我们并不总是要同时做这些操作。

// 我们把整个装配功能放在一个由大括号包围的代码块中，这样我们就可以用一个 TimerOutput::Scope 变量来衡量在这个代码块中花费了多少时间，不包括在这个函数中发生在匹配的闭合括号`}`之后的一切。

  template <int dim>
  void MinimalSurfaceProblem<dim>::compute_and_factorize_jacobian(
    const Vector<double> &evaluation_point)
  {
    {
      TimerOutput::Scope t(computing_timer, "assembling the Jacobian");

      std::cout << "  Computing Jacobian matrix" << std::endl;

      const QGauss<dim> quadrature_formula(fe.degree + 1);

      jacobian_matrix = 0;

      FEValues<dim> fe_values(fe,
                              quadrature_formula,
                              update_gradients | update_quadrature_points |
                                update_JxW_values);

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

      FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);

      std::vector<Tensor<1, dim>> evaluation_point_gradients(n_q_points);

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

      for (const auto &cell : dof_handler.active_cell_iterators())
        {
          cell_matrix = 0;

          fe_values.reinit(cell);

          fe_values.get_function_gradients(evaluation_point,
                                           evaluation_point_gradients);

          for (unsigned int q = 0; q < n_q_points; ++q)
            {
              const double coeff =
                1.0 / std::sqrt(1 + evaluation_point_gradients[q] *
                                      evaluation_point_gradients[q]);

              for (unsigned int i = 0; i < dofs_per_cell; ++i)
                {
                  for (unsigned int j = 0; j < dofs_per_cell; ++j)
                    cell_matrix(i, j) +=
                      (((fe_values.shape_grad(i, q)    // ((\nabla \phi_i
                         * coeff                       //   * a_n
                         * fe_values.shape_grad(j, q)) //   * \nabla \phi_j)
                        -                              //  -
                        (fe_values.shape_grad(i, q)    //  (\nabla \phi_i
                         * coeff * coeff * coeff       //   * a_n^3
                         *
                         (fe_values.shape_grad(j, q)       //   * (\nabla \phi_j
                          * evaluation_point_gradients[q]) //      * \nabla u_n)
                         * evaluation_point_gradients[q])) //   * \nabla u_n)))
                       * fe_values.JxW(q));                // * dx
                }
            }

          cell->get_dof_indices(local_dof_indices);
          hanging_node_constraints.distribute_local_to_global(cell_matrix,
                                                              local_dof_indices,
                                                              jacobian_matrix);
        }

      std::map<types::global_dof_index, double> boundary_values;
      VectorTools::interpolate_boundary_values(dof_handler,
                                               0,
                                               Functions::ZeroFunction<dim>(),
                                               boundary_values);
      Vector<double> dummy_solution(dof_handler.n_dofs());
      Vector<double> dummy_rhs(dof_handler.n_dofs());
      MatrixTools::apply_boundary_values(boundary_values,
                                         jacobian_matrix,
                                         dummy_solution,
                                         dummy_rhs);
    }

// 该函数的后半部分是对计算出的矩阵进行因数分解。为此，我们首先创建一个新的SparseDirectUMFPACK对象，并将其分配给成员变量`jacobian_matrix_factorization`，同时销毁该指针之前指向的任何对象（如果有）。然后我们告诉该对象对雅各布系数进行分解。

// 如上所述，我们把这段代码放在大括号里，用一个计时器来评估这部分程序所需的时间。

// (严格来说，我们在这里完成后实际上不再需要矩阵了，我们可以把矩阵对象扔掉。一个旨在提高内存效率的代码会这样做，并且只在这个函数中创建矩阵对象，而不是作为周围类的成员变量。我们在这里省略了这一步，因为使用与以前的教程程序相同的编码风格可以培养对通用风格的熟悉，并有助于使这些教程程序更容易阅读)。

    {
      TimerOutput::Scope t(computing_timer, "factorizing the Jacobian");

      std::cout << "  Factorizing Jacobian matrix" << std::endl;

      jacobian_matrix_factorization = std::make_unique<SparseDirectUMFPACK>();
      jacobian_matrix_factorization->factorize(jacobian_matrix);
    }
  }

//  @sect4{Computing the residual vector}

// `assemble_system()`在 step-15 中用来做的第二部分是计算残差向量，也就是牛顿线性系统的右手向量。我们把这一点从前面的函数中分解出来，但如果你理解了 step-15 中`assemble_system()`的作用，下面的函数就会很容易理解。然而，重要的是，我们需要计算的残差不是围绕当前解向量线性化的，而是我们从KINSOL得到的任何东西。这对于诸如直线搜索这样的操作是必要的，我们想知道在不同的 $\alpha_k$ 值下，残差 $F(U^k + \alpha_k \delta U^K)$ 是多少；在这些情况下，KINSOL只是给我们函数 $F$ 的参数，然后我们在这时计算残差 $F(\cdot)$ 。

// 该函数在最后打印出如此计算的残差的规范，作为我们跟踪程序进展的一种方式。

  template <int dim>
  void MinimalSurfaceProblem<dim>::compute_residual(
    const Vector<double> &evaluation_point,
    Vector<double> &      residual)
  {
    TimerOutput::Scope t(computing_timer, "assembling the residual");

    std::cout << "  Computing residual vector..." << std::flush;

    const QGauss<dim> quadrature_formula(fe.degree + 1);
    FEValues<dim>     fe_values(fe,
                            quadrature_formula,
                            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();

    Vector<double>              cell_residual(dofs_per_cell);
    std::vector<Tensor<1, dim>> evaluation_point_gradients(n_q_points);

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

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

        fe_values.get_function_gradients(evaluation_point,
                                         evaluation_point_gradients);

        for (unsigned int q = 0; q < n_q_points; ++q)
          {
            const double coeff =
              1.0 / std::sqrt(1 + evaluation_point_gradients[q] *
                                    evaluation_point_gradients[q]);

            for (unsigned int i = 0; i < dofs_per_cell; ++i)
              cell_residual(i) = (fe_values.shape_grad(i, q) // \nabla \phi_i
                                  * coeff                    // * a_n
                                  * evaluation_point_gradients[q] // * u_n
                                  * fe_values.JxW(q));            // * dx
          }

        cell->get_dof_indices(local_dof_indices);
        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          residual(local_dof_indices[i]) += cell_residual(i);
      }

    hanging_node_constraints.condense(residual);

    for (const types::global_dof_index i :
         DoFTools::extract_boundary_dofs(dof_handler))
      residual(i) = 0;

    for (const types::global_dof_index i :
         DoFTools::extract_hanging_node_dofs(dof_handler))
      residual(i) = 0;

    std::cout << " norm=" << residual.l2_norm() << std::endl;
  }

//  @sect4{Solving linear systems with the Jacobian matrix}

// 接下来是实现用雅各布矩阵解线性系统的函数。由于我们在建立矩阵时已经对矩阵进行了因式分解，所以解决线性系统的方法就是将逆矩阵应用于给定的右侧向量。这就是我们在这里使用的 SparseDirectUMFPACK::vmult() 函数的作用。在这之后，我们必须确保我们也能解决解向量中的悬空节点的值，而这是用 AffineConstraints::distribute(). 来完成的。

// 该函数需要一个额外的，但未使用的参数`tolerance`，它表示我们必须解决线性系统的精确程度。这个参数的含义在介绍中结合 "Eisenstat Walker技巧 "进行了讨论，但由于我们使用的是直接求解器而不是迭代求解器，所以我们并没有利用这个机会只求解线性系统的不精确性。

  template <int dim>
  void MinimalSurfaceProblem<dim>::solve(const Vector<double> &rhs,
                                         Vector<double> &      solution,
                                         const double /*tolerance*/)
  {
    TimerOutput::Scope t(computing_timer, "linear system solve");

    std::cout << "  Solving linear system" << std::endl;

    jacobian_matrix_factorization->vmult(solution, rhs);

    hanging_node_constraints.distribute(solution);
  }

//  @sect4{Refining the mesh, setting boundary values, and generating graphical output}

// 以下三个函数又是对  step-15  中的函数的简单复制。

  template <int dim>
  void MinimalSurfaceProblem<dim>::refine_mesh()
  {
    Vector<float> estimated_error_per_cell(triangulation.n_active_cells());

    KellyErrorEstimator<dim>::estimate(
      dof_handler,
      QGauss<dim - 1>(fe.degree + 1),
      std::map<types::boundary_id, const Function<dim> *>(),
      current_solution,
      estimated_error_per_cell);

    GridRefinement::refine_and_coarsen_fixed_number(triangulation,
                                                    estimated_error_per_cell,
                                                    0.3,
                                                    0.03);

    triangulation.prepare_coarsening_and_refinement();

    SolutionTransfer<dim> solution_transfer(dof_handler);
    solution_transfer.prepare_for_coarsening_and_refinement(current_solution);

    triangulation.execute_coarsening_and_refinement();

    dof_handler.distribute_dofs(fe);

    Vector<double> tmp(dof_handler.n_dofs());
    solution_transfer.interpolate(current_solution, tmp);
    current_solution = std::move(tmp);

    hanging_node_constraints.clear();

    DoFTools::make_hanging_node_constraints(dof_handler,
                                            hanging_node_constraints);
    hanging_node_constraints.close();

    hanging_node_constraints.distribute(current_solution);

    set_boundary_values();

    setup_system(/*initial_step=*/false);
  }

  template <int dim>
  void MinimalSurfaceProblem<dim>::set_boundary_values()
  {
    std::map<types::global_dof_index, double> boundary_values;
    VectorTools::interpolate_boundary_values(dof_handler,
                                             0,
                                             BoundaryValues<dim>(),
                                             boundary_values);
    for (const auto &boundary_value : boundary_values)
      current_solution(boundary_value.first) = boundary_value.second;

    hanging_node_constraints.distribute(current_solution);
  }

  template <int dim>
  void MinimalSurfaceProblem<dim>::output_results(
    const unsigned int refinement_cycle)
  {
    TimerOutput::Scope t(computing_timer, "graphical output");

    DataOut<dim> data_out;

    data_out.add_data_vector(current_solution, "solution");
    data_out.build_patches();

    const std::string filename =
      "solution-" + Utilities::int_to_string(refinement_cycle, 2) + ".vtu";
    std::ofstream output(filename);
    data_out.write_vtu(output);
  }

//  @sect4{The run() function and the overall logic of the program}

// 这个程序中唯一**有趣的函数是驱动整个算法的函数，即从一个粗大的网格开始，做一些网格细化循环，并在每个网格上使用KINSOL来寻找我们从这个网格上离散化得到的非线性代数方程的解。上面的`refine_mesh()`函数可以确保一个网格上的解被用作下一个网格的起始猜测。我们还使用一个TimerOutput对象来测量每个网格上的每一次操作所花费的时间，并在每个周期开始时重置该计时器。

// 正如在介绍中所讨论的，没有必要特别精确地解决粗略网格上的问题，因为这些问题只能作为下一个网格的起始猜测来解决。因此，我们将在 $k$ 个网格细化周期中使用 $\tau=10^{-3} \frac{1}{10^k}$ 的目标公差。

// 所有这些都在这个函数的第一部分进行了编码。

  template <int dim>
  void MinimalSurfaceProblem<dim>::run()
  {
    GridGenerator::hyper_ball(triangulation);
    triangulation.refine_global(2);

    setup_system(/*initial_step=*/true);
    set_boundary_values();

    for (unsigned int refinement_cycle = 0; refinement_cycle < 6;
         ++refinement_cycle)
      {
        computing_timer.reset();
        std::cout << "Mesh refinement step " << refinement_cycle << std::endl;

        if (refinement_cycle != 0)
          refine_mesh();

        const double target_tolerance = 1e-3 * std::pow(0.1, refinement_cycle);
        std::cout << "  Target_tolerance: " << target_tolerance << std::endl
                  << std::endl;

// 这就是有趣的开始。在顶部，我们创建了KINSOL求解器对象，并给它提供了一个对象，该对象编码了一些额外的具体情况（其中我们只改变了我们想要达到的非线性容忍度；但你可能想看看 SUNDIALS::KINSOL::AdditionalData 类有哪些其他成员，并与它们一起玩）。

        {
          typename SUNDIALS::KINSOL<Vector<double>>::AdditionalData
            additional_data;
          additional_data.function_tolerance = target_tolerance;

          SUNDIALS::KINSOL<Vector<double>> nonlinear_solver(additional_data);

// 然后，我们必须描述在介绍中已经提到的操作。从本质上讲，我们必须教KINSOL如何(i)将一个向量调整到正确的大小，(ii)计算残差向量，(iii)计算雅各布矩阵（在这期间我们也计算其因式分解），以及(iv)用雅各布矩阵解一个线性系统。

// 所有这四种操作都由 SUNDIALS::KINSOL 类的成员变量表示，这些成员变量的类型是 `std::function`, ，即它们是我们可以分配给一个函数的指针的对象，或者像我们在这里做的那样，一个 "lambda函数"，它接受相应的参数并返回相应的信息。按照惯例，KINSOL希望做一些不重要的事情的函数返回一个整数，其中0表示成功。事实证明，我们只需用25行代码就可以完成所有这些工作。

// 如果你不知道什么是 "lambda函数"，可以看看 step-12 或[wikipedia页面](https:en.wikipedia.org/wiki/Anonymous_function)关于这个问题。lambda函数的想法是，人们想用一组参数来定义一个函数，但(i)不使它成为一个命名的函数，因为通常情况下，该函数只在一个地方使用，似乎没有必要给它一个全局名称；(ii)该函数可以访问存在于定义它的地方的一些变量，包括成员变量。lambda函数的语法很笨拙，但最终还是很有用的）。)

// 在代码块的最后，我们告诉KINSOL去工作，解决我们的问题。从'residual'、'setup_jacobian'和'solve_jacobian_system'函数中调用的成员函数将向屏幕打印输出，使我们能够跟踪程序的进展情况。

          nonlinear_solver.reinit_vector = [&](Vector<double> &x) {
            x.reinit(dof_handler.n_dofs());
          };

          nonlinear_solver.residual =
            [&](const Vector<double> &evaluation_point,
                Vector<double> &      residual) {
              compute_residual(evaluation_point, residual);

              return 0;
            };

          nonlinear_solver.setup_jacobian =
            [&](const Vector<double> &current_u,
                const Vector<double> & /*current_f*/) {
              compute_and_factorize_jacobian(current_u);

              return 0;
            };

          nonlinear_solver.solve_with_jacobian = [&](const Vector<double> &rhs,
                                                     Vector<double> &      dst,
                                                     const double tolerance) {
            this->solve(rhs, dst, tolerance);

            return 0;
          };

          nonlinear_solver.solve(current_solution);
        }

// 剩下的就只是内务整理了。将数据写入文件，以便进行可视化，并显示收集到的时间摘要，以便我们可以解释每个操作花了多长时间，执行的频率如何，等等。

        output_results(refinement_cycle);

        computing_timer.print_summary();

        std::cout << std::endl;
      }
  }
} // namespace Step77

int main()
{
  try
    {
      using namespace Step77;

      MinimalSurfaceProblem<2> laplace_problem_2d;
      laplace_problem_2d.run();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

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


/**
 * \file boost/numeric/ublasx/operation/min.hpp
 *
 * \brief The \c min operation.
 *
 * <hr/>
 *
 * Copyright (c) 2010-2011, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_MIN_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_MIN_HPP

#include <boost/numeric/ublas/detail/config.hpp>
#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublasx/detail/debug.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/size.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <complex>
#include <cstddef>
#include <limits>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

//@{ Declarations

/**
 * \brief Find the minimum element of the given vector expression.
 * \tparam VectorExprT The type of the vector expression.
 * \param ve The vector expression over which to iterate for finding the minimum
 *  element.
 * \return The minimum element in the vector expression.
 *
 * \author Marco Guazzone, <marco.guazzone@gmail.com>
 */
template <typename VectorExprT>
typename vector_traits<VectorExprT>::value_type min(vector_expression<VectorExprT> const& ve);

/**
 * \brief Find the minimum element of the given matrix expression.
 * \tparam MatrixExprT The type of the matrix expression.
 * \param me The matrix expression over which to iterate for finding the minimum
 *  element.
 * \return The minimum element in the matrix expression.
 *
 * \author Marco Guazzone, <marco.guazzone@gmail.com>
 */
template <typename MatrixExprT>
typename matrix_traits<MatrixExprT>::value_type min(matrix_expression<MatrixExprT> const& me);

/**
 * \brief Find the minimum element of each row in the given matrix expression.
 * \tparam MatrixExprT The type of the matrix expression.
 * \param me The matrix expression over which to iterate for finding the minimum
 *  element of each row.
 * \return A vector containing the minimum element for each row in the given
 *  matrix expression.
 *
 * \author Marco Guazzone, <marco.guazzone@gmail.com>
 */
template <typename MatrixExprT>
vector<typename matrix_traits<MatrixExprT>::value_type> min_rows(matrix_expression<MatrixExprT> const& me);

/**
 * \brief Find the minimum element of each column in the given matrix
 *  expression.
 * \tparam MatrixExprT The type of the matrix expression.
 * \param me The matrix expression over which to iterate for finding the minimum
 *  element of each column.
 * \return A vector containing the minimum element for each column in the given
 *  matrix expression.
 *
 * \author Marco Guazzone, <marco.guazzone@gmail.com>
 */
template <typename MatrixExprT>
vector<typename matrix_traits<MatrixExprT>::value_type> min_columns(matrix_expression<MatrixExprT> const& me);

/**
 * \brief Find the minimum element of the given vector expression.
 * \tparam Dim The dimension number (for vector, only dimension 1 is valid).
 * \tparam MatrixExprT The type of the vectovector expression.
 * \param ve The vector expression over which to iterate for finding the minimum
 *  element over the given dimension.
 * \return A vector of size 1 containing the minimum element of the given vector
 *  expression.
 *
 * This function is provided for the sake of usability, in order to make to make
 * the call to \c size<1>(vec) a valid call.
 * For the same reason, the return type is a vector instead of a simple scalar.
 *
 * \author Marco Guazzone, <marco.guazzone@gmail.com>
 */
template <size_t Dim, typename VectorExprT>
vector<typename vector_traits<VectorExprT>::value_type> min(vector_expression<VectorExprT> const& ve);

/**
 * \brief Find the minimum elements over the given dimension of the given matrix
 *  expression.
 * \tparam Dim The dimension number (starting from 1).
 * \tparam MatrixExprT The type of the matrix expression.
 * \param me The matrix expression over which to iterate for finding the minimum
 *  element over the given dimension.
 * \return A vector containing the minimum elements over the given dimension in
 *  the given matrix expression.
 *
 * \author Marco Guazzone, <marco.guazzone@gmail.com>
 */
template <size_t Dim, typename MatrixExprT>
vector<typename matrix_traits<MatrixExprT>::value_type> min(matrix_expression<MatrixExprT> const& me);

/**
 * \brief Find the minimum elements over the given dimension tag of the given
 *  matrix expression.
 * \tparam TagT The dimension tag type (e.g., tag::major).
 * \tparam MatrixExprT The type of the matrix expression.
 * \param me The matrix expression over which to iterate for finding the minimum
 *  element over the given dimension.
 * \return A vector containing the minimum elements over the given dimension in
 *  the given matrix expression.
 *
 * \author Marco Guazzone, <marco.guazzone@gmail.com>
 */
template <typename TagT, typename MatrixExprT>
vector<typename matrix_traits<MatrixExprT>::value_type> min_by_tag(matrix_expression<MatrixExprT> const& me);

//@} Declarations

namespace detail { namespace /*<unnamed>*/ {

//@{ Declarations

/// Helper function for implementing the 'less-than' relational operator over
/// generic types.
template <typename T>
bool less_than_impl(T a, T b);

/// Helper function for implementing the 'less-than' relational operator over
/// complex types.
template <typename T>
bool less_than_impl(::std::complex<T> const& a, ::std::complex<T> const& b);

/**
 * \brief Helper class for real/complex infinity
 *
 * See Wolfram MathWorld (http://mathworld.wolfram.com/ComplexInfinity.html)
 * for a definition of complex infinity.
 */
template <typename T>
struct infinity;

/**
 * \brief Auxiliary class for computing the minimum elements over the given
 *  dimension for a container of the given category.
 * \tparam Dim The dimension number (starting from 1).
 * \tparam CategoryT The category type (e.g., vector_tag).
 */
template <std::size_t Dim, typename CategoryT>
struct min_by_dim_impl;

/**
 * \brief Auxiliary class for computing the minimum elements over the given
 *  dimension tag for a container of the given category.
 * \tparam TagT The dimension tag type (e.g., tag::major).
 * \tparam CategoryT The category type (e.g., vector_tag).
 * \tparam OrientationT The orientation category type (e.g., row_major_tag).
 */
template <typename TagT, typename CategoryT, typename OrientationT>
struct min_by_tag_impl;

//@} Declarations

//@{ Definitions

template <typename T>
BOOST_UBLAS_INLINE
bool less_than_impl(T a, T b)
{
	return a < b;
}

template <typename T>
BOOST_UBLAS_INLINE
bool less_than_impl(::std::complex<T> const& a, ::std::complex<T> const& b)
{
	// For complex numbers compare modulus and phase angle
	// Use the same logic used by MATLAB for the 'min' function:
	// "For complex input A, min returns the complex number with the smallest
	//  complex modulus (magnitude), computed with min(abs(A)). Then
	//  computes the smallest phase angle with min(angle(x)), if necessary"

	const T ax(::std::abs(a));
	const T bx(::std::abs(b));
	return ax < bx
		   || (ax == bx && ::std::arg(a) < ::std::arg(b));
}

template <typename T>
struct infinity
{
	static const T value;
};
template <typename T>
const T infinity<T>::value = ::std::numeric_limits<T>::has_infinity ? ::std::numeric_limits<T>::infinity() : ::std::numeric_limits<T>::max();

template <typename T>
struct infinity< ::std::complex<T> >
{
	static const ::std::complex<T> value;
};
template <typename T>
const ::std::complex<T> infinity< ::std::complex<T> >::value = ::std::complex<T>(infinity<T>::value,::std::numeric_limits<T>::quiet_NaN());

template <>
struct min_by_dim_impl<1, vector_tag>
{
	template <typename VectorExprT>
	BOOST_UBLAS_INLINE
	static vector<typename vector_traits<VectorExprT>::value_type> apply(vector_expression<VectorExprT> const& ve)
	{
		typedef typename vector_traits<VectorExprT>::value_type value_type;

		vector<value_type> res(1);

		res(0) = min(ve);

		return res;
	}
};

template <>
struct min_by_dim_impl<1, matrix_tag>
{
	template <typename MatrixExprT>
	BOOST_UBLAS_INLINE
	static vector<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
	{
		return min_rows(me);
	}
};

template <>
struct min_by_dim_impl<2, matrix_tag>
{
	template <typename MatrixExprT>
	BOOST_UBLAS_INLINE
	static vector<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
	{
		return min_columns(me);
	}
};

template <>
struct min_by_tag_impl<tag::major, matrix_tag, row_major_tag>
{
	template <typename MatrixExprT>
	BOOST_UBLAS_INLINE
	static vector<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
	{
		return min_rows(me);
	}
};

template <>
struct min_by_tag_impl<tag::minor, matrix_tag, row_major_tag>
{
	template <typename MatrixExprT>
	BOOST_UBLAS_INLINE
	static vector<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
	{
		return min_columns(me);
	}
};

template <>
struct min_by_tag_impl<tag::leading, matrix_tag, row_major_tag>
{
	template <typename MatrixExprT>
	BOOST_UBLAS_INLINE
	static vector<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
	{
		return min_columns(me);
	}
};

template <>
struct min_by_tag_impl<tag::major, matrix_tag, column_major_tag>
{
	template <typename MatrixExprT>
	BOOST_UBLAS_INLINE
	static vector<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
	{
		return min_columns(me);
	}
};

template <>
struct min_by_tag_impl<tag::minor, matrix_tag, column_major_tag>
{
	template <typename MatrixExprT>
	BOOST_UBLAS_INLINE
	static vector<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
	{
		return min_rows(me);
	}
};

template <>
struct min_by_tag_impl<tag::leading, matrix_tag, column_major_tag>
{
	template <typename MatrixExprT>
	BOOST_UBLAS_INLINE
	static vector<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
	{
		return min_rows(me);
	}
};

template <typename TagT>
struct min_by_tag_impl<TagT, matrix_tag, unknown_orientation_tag>: min_by_tag_impl<TagT, matrix_tag, row_major_tag>
{
	// Empty
};

//@} Definitions

}} // Namespace detail::<unnamed>

//@{ Definitions

template <typename VectorExprT>
BOOST_UBLAS_INLINE
typename vector_traits<VectorExprT>::value_type min(vector_expression<VectorExprT> const& ve)
{
	typedef typename vector_traits<VectorExprT>::size_type size_type;
	typedef typename vector_traits<VectorExprT>::value_type value_type;

	size_type n = size(ve);
/*
	value_type m = ::std::numeric_limits<value_type>::has_infinity
				   ? ::std::numeric_limits<value_type>::infinity()
				   : ::std::numeric_limits<value_type>::max();
*/
	value_type m = detail::infinity<value_type>::value;

	for (size_type i = 0; i < n; ++i)
	{
//		if (ve()(i) < m)
		if (detail::less_than_impl(ve()(i), m))
		{
			m = ve()(i);
		}
	}

	return m;
}

template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename matrix_traits<MatrixExprT>::value_type min(matrix_expression<MatrixExprT> const& me)
{
	typedef typename matrix_traits<MatrixExprT>::size_type size_type;
	typedef typename matrix_traits<MatrixExprT>::value_type value_type;

	size_type nr = num_rows(me);
	size_type nc = num_columns(me);
//	value_type m = ::std::numeric_limits<value_type>::has_infinity
//				   ? ::std::numeric_limits<value_type>::infinity()
//				   : ::std::numeric_limits<value_type>::max();
	value_type m = detail::infinity<value_type>::value;

	for (size_type r = 0; r < nr; ++r)
	{
		for (size_type c = 0; c < nc; ++c)
		{
//			if (me()(r,c) < m)
			if (detail::less_than_impl(me()(r,c), m))
			{
				m = me()(r,c);
			}
		}
	}

	return m;
}

template <typename MatrixExprT>
BOOST_UBLAS_INLINE
vector<typename matrix_traits<MatrixExprT>::value_type> min_rows(matrix_expression<MatrixExprT> const& me)
{
	typedef typename matrix_traits<MatrixExprT>::size_type size_type;
	typedef typename matrix_traits<MatrixExprT>::value_type value_type;

	size_type nr = num_rows(me);
	size_type nc = num_columns(me);

	vector<value_type> res(nr);
	size_type j = 0;
	for (size_type r = 0; r < nr; ++r)
	{
//		value_type m = ::std::numeric_limits<value_type>::has_infinity
//					   ? ::std::numeric_limits<value_type>::infinity()
//					   : ::std::numeric_limits<value_type>::max();
		value_type m = detail::infinity<value_type>::value;

		for (size_type c = 0; c < nc; ++c)
		{
//			if (me()(r,c) < m)
			if (detail::less_than_impl(me()(r,c), m))
			{
				m = me()(r,c);
			}
		}

		res(j++) = m;
	}

	return res;
}

template <typename MatrixExprT>
BOOST_UBLAS_INLINE
vector<typename matrix_traits<MatrixExprT>::value_type> min_columns(matrix_expression<MatrixExprT> const& me)
{
	typedef typename matrix_traits<MatrixExprT>::size_type size_type;
	typedef typename matrix_traits<MatrixExprT>::value_type value_type;

	size_type nr = num_rows(me);
	size_type nc = num_columns(me);

	vector<value_type> res(nc);
	size_type j = 0;
	for (size_type c = 0; c < nc; ++c)
	{
//		value_type m = ::std::numeric_limits<value_type>::has_infinity
//					   ? ::std::numeric_limits<value_type>::infinity()
//					   : ::std::numeric_limits<value_type>::max();
		value_type m = detail::infinity<value_type>::value;

		for (size_type r = 0; r < nr; ++r)
		{
//			if (me()(r,c) < m)
			if (detail::less_than_impl(me()(r,c), m))
			{
				m = me()(r,c);
			}
		}

		res(j++) = m;
	}

	return res;
}

template <size_t Dim, typename VectorExprT>
BOOST_UBLAS_INLINE
vector<typename vector_traits<VectorExprT>::value_type> min(vector_expression<VectorExprT> const& ve)
{
	return detail::min_by_dim_impl<Dim, vector_tag>::template apply(ve);
}

template <size_t Dim, typename MatrixExprT>
BOOST_UBLAS_INLINE
vector<typename matrix_traits<MatrixExprT>::value_type> min(matrix_expression<MatrixExprT> const& me)
{
	return detail::min_by_dim_impl<Dim, matrix_tag>::template apply(me);
}

template <typename TagT, typename MatrixExprT>
//template <typename MatrixExprT, typename TagT>
BOOST_UBLAS_INLINE
vector<typename matrix_traits<MatrixExprT>::value_type> min_by_tag(matrix_expression<MatrixExprT> const& me)
{
	return detail::min_by_tag_impl<TagT, matrix_tag, typename matrix_traits<MatrixExprT>::orientation_category>::template apply(me);
}

//@} Definitions

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_MIN_HPP


#include "plugin_graph_algorithm.h"

#include "core/log.h"

#include "netlist/gate.h"
#include "netlist/net.h"
#include "netlist/netlist.h"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/graph_traits.hpp>

std::map<std::shared_ptr<gate>, std::tuple<std::vector<std::shared_ptr<gate>>, int>> plugin_graph_algorithm::get_dijkstra_shortest_paths(const std::shared_ptr<gate> g)
{
    if (g == nullptr)
    {
        log_error(this->get_name(), "parameter 'g' is nullptr");
        return {};
    }

    /*
     * boost graph definition for a directed graph
     *
     * boost::adjacency_list - adjacency list
     * boost::listS - stores edge set of each vertex in a list
     * boost::vecS - stores the vertex set in a vector
     * boost::directedS - graph is directed
     * boost::no_property - no information stored to vertex
     * boost::property<boost::edge_weight_t, int> - weight information stored to edge
     */
    typedef boost::adjacency_list<boost::listS, boost::vecS, boost::directedS, boost::no_property, boost::property<boost::edge_weight_t, int>> boost_graph_t;

    /*
     * boost vertex definition for the boost_graph_t
     */
    typedef boost::graph_traits<boost_graph_t>::vertex_descriptor vertex_t;

    boost_graph_t boost_graph;

    // add gates ordered by id (for determinisitc behavior) to boost graph
    // vertices in boost graph are ordered from 0, 1, ...
    auto nl = g->get_netlist();
    std::set<u32> gate_ids;
    for (const auto& gate : nl->get_gates())
        gate_ids.insert(gate->get_id());

    std::map<u32, vertex_t> gate_id_to_vertex;
    std::map<vertex_t, u32> vertex_id_to_gate_id;

    u32 vertex_id_cnt = 0;
    for (const auto& gate_id : gate_ids)
    {
        auto vd                               = boost::add_vertex(boost_graph);
        gate_id_to_vertex[gate_id]            = vd;
        vertex_id_to_gate_id[vertex_id_cnt++] = gate_id;
    }

    // add ordered weigthened edges (weight = 1) to directed boost graph
    std::map<u32, std::shared_ptr<net>> ordered_nets;
    for (const auto& net : nl->get_nets())
        ordered_nets[net->get_id()] = net;

    for (const auto& it : ordered_nets)
    {
        if (it.second->get_src().gate == nullptr)
            continue;

        std::set<u32> dst_ids;
        for (auto dst : it.second->get_dsts())
            dst_ids.insert(dst.gate->get_id());

        for (const auto& dst_id : dst_ids)
            boost::add_edge(gate_id_to_vertex[it.second->get_src().gate->get_id()], gate_id_to_vertex[dst_id], 1, boost_graph);
    }

    // initialize parameters for dijkstra_shortest_paths()
    std::vector<vertex_t> predecessors(nl->get_gates().size());
    std::vector<int> distance(nl->get_gates().size());

    dijkstra_shortest_paths(boost_graph,
                            gate_id_to_vertex[g->get_id()],
                            predecessor_map(boost::make_iterator_property_map(predecessors.begin(), get(boost::vertex_index, boost_graph)))
                                .distance_map(boost::make_iterator_property_map(distance.begin(), get(boost::vertex_index, boost_graph))));

    // postprocess boost result
    std::map<std::shared_ptr<gate>, std::tuple<std::vector<std::shared_ptr<gate>>, int>> result;
    boost::graph_traits<boost_graph_t>::vertex_iterator vi, vend;
    for (boost::tie(vi, vend) = boost::vertices(boost_graph); vi != vend; ++vi)
    {
        if (distance[*vi] == ((i64)1 << 31) - 1)
        {
            // no path from g to gate
            result[nl->get_gate_by_id(vertex_id_to_gate_id[*vi])] = std::make_tuple(std::vector<std::shared_ptr<gate>>(), -1);
        }
        else
        {
            // path from src to gate, so assemble path
            std::vector<std::shared_ptr<gate>> path;
            auto tmp = *vi;
            while (vertex_id_to_gate_id[tmp] != g->get_id())
            {
                path.push_back(nl->get_gate_by_id(vertex_id_to_gate_id[tmp]));
                tmp = predecessors[tmp];
            }
            path.push_back(g);
            std::reverse(path.begin(), path.end());
            result[nl->get_gate_by_id(vertex_id_to_gate_id[*vi])] = std::make_tuple(path, distance[*vi]);
        }
    }
    return result;
}


#include <ceres/ceres.h>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <math.h>
#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#include <thread>
using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solve;
using ceres::Solver;
const int kNumObservations = 8;

#define STRINGIFY(x) #x
#define MACRO_STRINGIFY(x) STRINGIFY(x)
namespace py = pybind11;

// -------------
// pure C++ code
// -------------
struct Cost_FixedM_1mag {
  Cost_FixedM_1mag(double x, double y, double z, double x2, double y2,
                   double z2, double m)
      : Bx(x),
        By(y),
        Bz(z),
        Xs(x2),
        Ys(y2),
        Zs(z2),
        M{m} {}  // init the sensor position and the magnitude reading.
  template <typename T>
  bool operator()(const T *const x, const T *const y, const T *const z,
                  const T *const theta, const T *const phy, const T *const Gx,
                  const T *const Gy, const T *const Gz, T *residual)
      const {  // x y z is the coordinates of magnate j, m is the attributes of
               // magate j, theta phy is the orientation of the magnate
    Eigen::Matrix<T, 3, 1> VecM =
        Eigen::Matrix<T, 3, 1>(sin(theta[0]) * cos(phy[0]),
                               sin(theta[0]) * sin(phy[0]), cos(theta[0])) *
        1e-7 * exp(M);
    Eigen::Matrix<T, 3, 1> VecR =
        Eigen::Matrix<T, 3, 1>(Xs - x[0], Ys - y[0], Zs - z[0]);
    T NormR = VecR.norm();
    Eigen::Matrix<T, 3, 1> B =
        (3.0 * VecR * (VecM.transpose() * VecR) / pow(NormR, 5) -
         VecM /
             pow(NormR, 3));  // convert it's unit to correspond with the input
    // std::cout << "B= " << (B(0, 0) + Gx[0]) * 1e6 << "\t" << (B(1, 0) +
    // Gy[0]) * 1e6 << "\t" << (B(2, 0) + Gz[0]) * 1e6 << "\n"; std::cout <<
    // B(0) << '\n'
    //           << B(1) << '\n'
    //           << B(2) << std::endl;
    residual[0] = (B(0, 0) + Gx[0]) * 1e6 - Bx;
    residual[1] = (B(1, 0) + Gy[0]) * 1e6 - By;
    residual[2] = (B(2, 0) + Gz[0]) * 1e6 - Bz;
    // std::cout << residual[0] << '\t' << residual[1] << '\t' << residual[2] <<
    // std::endl;
    return true;
  }

 private:
  const double Bx;
  const double By;
  const double Bz;
  const double Xs;
  const double Ys;
  const double Zs;
  const double M;
};

struct Cost_FixedM_2mag {
  Cost_FixedM_2mag(double Bx_, double By_, double Bz_, double Xs_, double Ys_,
                   double Zs_, double m)
      : Bx(Bx_),
        By(By_),
        Bz(Bz_),
        Xs(Xs_),
        Ys(Ys_),
        Zs(Zs_),
        M{m} {}  // init the sensor position and the magnitude reading.
  template <typename T>
  bool operator()(const T *const Gx, const T *const Gy, const T *const Gz,
                  const T *const x0, const T *const y0, const T *const z0,
                  const T *const theta0, const T *const phy0, const T *const x1,
                  const T *const y1, const T *const z1, const T *const theta1,
                  const T *const phy1, T *residual)
      const {  // x y z is the coordinates of magnate j, m is the attributes of
               // magate j, theta phy is the orientation of the magnate
    // mag one
    Eigen::Matrix<T, 3, 1> VecM0 =
        Eigen::Matrix<T, 3, 1>(sin(theta0[0]) * cos(phy0[0]),
                               sin(theta0[0]) * sin(phy0[0]), cos(theta0[0])) *
        1e-7 * exp(M);
    Eigen::Matrix<T, 3, 1> VecR0 =
        Eigen::Matrix<T, 3, 1>(Xs - x0[0], Ys - y0[0], Zs - z0[0]);
    T NormR0 = VecR0.norm();
    Eigen::Matrix<T, 3, 1> B0 =
        (3.0 * VecR0 * (VecM0.transpose() * VecR0) / pow(NormR0, 5) -
         VecM0 /
             pow(NormR0, 3));  // convert it's unit to correspond with the input
    // mag two
    Eigen::Matrix<T, 3, 1> VecM1 =
        Eigen::Matrix<T, 3, 1>(sin(theta1[0]) * cos(phy1[0]),
                               sin(theta1[0]) * sin(phy1[0]), cos(theta1[0])) *
        1e-7 * exp(M);
    Eigen::Matrix<T, 3, 1> VecR1 =
        Eigen::Matrix<T, 3, 1>(Xs - x1[0], Ys - y1[0], Zs - z1[0]);
    T NormR1 = VecR1.norm();
    Eigen::Matrix<T, 3, 1> B1 =
        (3.0 * VecR1 * (VecM1.transpose() * VecR1) / pow(NormR1, 5) -
         VecM1 /
             pow(NormR1, 3));  // convert it's unit to correspond with the input

    residual[0] = (B0(0, 0) + B1(0, 0) + Gx[0]) * 1e6 - Bx;
    residual[1] = (B0(1, 0) + B1(1, 0) + Gy[0]) * 1e6 - By;
    residual[2] = (B0(2, 0) + B1(2, 0) + Gz[0]) * 1e6 - Bz;
    // std::cout << residual[0] << '\t' << residual[1] << '\t' << residual[2] <<
    // std::endl;
    return true;
  }

 private:
  const double Bx;
  const double By;
  const double Bz;
  const double Xs;
  const double Ys;
  const double Zs;
  const double M;
};

std::vector<double> cal_Bi(double xs, double ys, double zs,
                           std::vector<double> param) {
  double x = param[4];
  double y = param[5];
  double z = param[6];
  double theta = param[7];
  double phy = param[8];
  double Gx = param[0];
  double Gy = param[1];
  double Gz = param[2];
  double M = param[3];

  Eigen::Matrix<double, 3, 1> VecM =
      Eigen::Matrix<double, 3, 1>(sin(theta) * cos(phy), sin(theta) * sin(phy),
                                  cos(theta)) *
      1e-7 * M;
  Eigen::Matrix<double, 3, 1> VecR =
      Eigen::Matrix<double, 3, 1>(xs - x, ys - y, zs - z);
  double NormR = VecR.norm();
  Eigen::Matrix<double, 3, 1> B =
      (3.0 * VecR * (VecM.transpose() * VecR) / pow(NormR, 5) -
       VecM / pow(NormR, 3));  // convert it's unit to correspond with the input

  std::vector<double> reading = {(B(0, 0) + Gx) * 1e6, (B(1, 0) + Gy) * 1e6,
                                 (B(2, 0) + Gz) * 1e6};
  return reading;
}

class MagCost : public ceres::SizedCostFunction<3, 1, 1, 1, 1, 1, 1, 1, 1> {
 public:
  MagCost(const double Bx, const double By, const double Bz, const double Xs,
          const double Ys, const double Zs, const double M)
      : Bx_(Bx), By_(By), Bz_(Bz), Xs_(Xs), Ys_(Ys), Zs_(Zs), M_(M) {}
  virtual ~MagCost() {}
  virtual bool Evaluate(double const *const *parameters, double *residuals,
                        double **jacobians) const {
    double x = parameters[0][0];
    double y = parameters[1][0];
    double z = parameters[2][0];
    double theta = parameters[3][0];
    double phy = parameters[4][0];
    double Gx = parameters[5][0];
    double Gy = parameters[6][0];
    double Gz = parameters[7][0];

    Eigen::Matrix<double, 3, 1> VecM =
        Eigen::Matrix<double, 3, 1>(sin(theta) * cos(phy),
                                    sin(theta) * sin(phy), cos(theta)) *
        1e-7 * exp(M_);
    Eigen::Matrix<double, 3, 1> VecR =
        Eigen::Matrix<double, 3, 1>(Xs_ - x, Ys_ - y, Zs_ - z);
    double NormR = VecR.norm();
    Eigen::Matrix<double, 3, 1> B =
        (3.0 * VecR * (VecM.transpose() * VecR) / pow(NormR, 5) -
         VecM /
             pow(NormR, 3));  // convert it's unit to correspond with the input

    residuals[0] = (B(0, 0) + Gx) * 1e6 - Bx_;
    residuals[1] = (B(1, 0) + Gy) * 1e6 - By_;
    residuals[2] = (B(2, 0) + Gz) * 1e6 - Bz_;

    if (!jacobians) return true;

    // calculate dx
    double t1 = VecM.transpose() * VecR;
    jacobians[0][0] =
        1e6 * (5 * 3 * VecR(0, 0) * VecR(0, 0) * t1 / pow(NormR, 7) -
               (2 * 3 * VecR(0, 0) * VecM(0, 0) + 3 * t1) / pow(NormR, 5));
    jacobians[0][1] =
        1e6 * (5 * 3 * VecR(0, 0) * VecR(1, 0) * t1 / pow(NormR, 7) -
               (3 * VecR(0, 0) * VecM(1, 0) + 3 * VecR(1, 0) * VecM(0, 0)) /
                   pow(NormR, 5));
    jacobians[0][2] =
        1e6 * (5 * 3 * VecR(0, 0) * VecR(2, 0) * t1 / pow(NormR, 7) -
               (3 * VecR(0, 0) * VecM(2, 0) + 3 * VecR(2, 0) * VecM(0, 0)) /
                   pow(NormR, 5));

    // calculate dy
    jacobians[1][0] =
        1e6 * (5 * 3 * VecR(0, 0) * VecR(1, 0) * t1 / pow(NormR, 7) -
               (3 * VecR(0, 0) * VecM(1, 0) + 3 * VecR(1, 0) * VecM(0, 0)) /
                   pow(NormR, 5));
    jacobians[1][1] =
        1e6 * (5 * 3 * VecR(1, 0) * VecR(1, 0) * t1 / pow(NormR, 7) -
               (2 * 3 * VecR(1, 0) * VecM(1, 0) + 3 * t1) / pow(NormR, 5));
    jacobians[1][2] =
        1e6 * (5 * 3 * VecR(1, 0) * VecR(2, 0) * t1 / pow(NormR, 7) -
               (3 * VecR(1, 0) * VecM(2, 0) + 3 * VecR(2, 0) * VecM(1, 0)) /
                   pow(NormR, 5));

    // calculate dz
    jacobians[2][0] =
        1e6 * (5 * 3 * VecR(0, 0) * VecR(2, 0) * t1 / pow(NormR, 7) -
               (3 * VecR(0, 0) * VecM(2, 0) + 3 * VecR(2, 0) * VecM(0, 0)) /
                   pow(NormR, 5));
    jacobians[2][1] =
        1e6 * (5 * 3 * VecR(1, 0) * VecR(2, 0) * t1 / pow(NormR, 7) -
               (3 * VecR(1, 0) * VecM(2, 0) + 3 * VecR(2, 0) * VecM(1, 0)) /
                   pow(NormR, 5));
    jacobians[2][2] =
        1e6 * (5 * 3 * VecR(2, 0) * VecR(2, 0) * t1 / pow(NormR, 7) -
               (2 * 3 * VecR(2, 0) * VecM(2, 0) + 3 * t1) / pow(NormR, 5));

    // calculate d(theta)
    double t2 = 1e-7 * exp(M_) *
                (VecR(0, 0) * cos(phy) * cos(theta) +
                 VecR(1, 0) * sin(phy) * cos(theta) - VecR(2, 0) * sin(theta));
    jacobians[3][0] =
        1e6 * (3 * VecR(0, 0) * t2 / pow(NormR, 5) -
               1e-7 * exp(M_) * cos(phy) * cos(theta) / pow(NormR, 3));
    jacobians[3][1] =
        1e6 * (3 * VecR(1, 0) * t2 / pow(NormR, 5) -
               1e-7 * exp(M_) * sin(phy) * cos(theta) / pow(NormR, 3));
    jacobians[3][2] = 1e6 * (3 * VecR(2, 0) * t2 / pow(NormR, 5) +
                             1e-7 * exp(M_) * sin(theta) / pow(NormR, 3));

    // calculate d(phi)
    double t3 = 1e-7 * exp(M_) *
                (-VecR(0, 0) * sin(phy) * sin(theta) +
                 VecR(1, 0) * sin(theta) * cos(phy));
    jacobians[4][0] =
        1e6 * (3 * VecR(0, 0) * t3 / pow(NormR, 5) +
               1e-7 * exp(M_) * sin(phy) * sin(theta) / pow(NormR, 3));
    jacobians[4][1] =
        1e6 * (3 * VecR(1, 0) * t3 / pow(NormR, 5) -
               1e-7 * exp(M_) * cos(phy) * sin(theta) / pow(NormR, 3));
    jacobians[4][2] = 1e6 * (3 * VecR(2, 0) * t3 / pow(NormR, 5));

    // calculate dG
    jacobians[5][0] = 1e6;
    jacobians[5][1] = 0;
    jacobians[5][2] = 0;
    jacobians[6][0] = 0;
    jacobians[6][1] = 1e6;
    jacobians[6][2] = 0;
    jacobians[7][0] = 0;
    jacobians[7][1] = 0;
    jacobians[7][2] = 1e6;

    return true;
  }

 private:
  const double Bx_;
  const double By_;
  const double Bz_;
  const double Xs_;
  const double Ys_;
  const double Zs_;
  const double M_;
};

std::vector<double> solve_1mag(std::vector<double> readings,
                               std::vector<double> pSensor,
                               std::vector<double> init_param) {
  // std::vector<float> test_vector = { 2,1,3 };
  // Eigen::MatrixXf readings_vec = Eigen::Map<Eigen::Matrix<double, 8, 3>
  // >(readings.data()); Eigen::MatrixXf pSensor_vec =
  // Eigen::Map<Eigen::Matrix<double, 8, 3> >(pSensor.data());
  Eigen::VectorXd readings_vec = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
      readings.data(), readings.size());
  Eigen::VectorXd pSensor_vec = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
      pSensor.data(), pSensor.size());
  // Eigen::MatrixXd readings_vec_1(&readings[0], 8, 3);
  // Eigen::MatrixXd pSensor_vec_1(&pSensor[0], 8, 3);
  // Eigen::Map<Eigen::MatrixXd> readings_vec(readings_vec_1.data(), 3, 8);
  // Eigen::Map<Eigen::MatrixXd> pSensor_vec(pSensor_vec_1.data(), 3, 8);
  // readings_vec = readings_vec.transpose();
  // pSensor_vec = pSensor_vec.transpose();
  // std::cout
  //     << "readings_vec: " << readings_vec << "\n";
  // std::cout << "pSensor_vec: " << pSensor_vec << "\n";

  double Gx = init_param[0];
  double Gy = init_param[1];
  double Gz = init_param[2];
  double m = init_param[3];
  double x = init_param[4];
  double y = init_param[5];
  double z = init_param[6];
  double theta = init_param[7];
  double phy = init_param[8];
  // std::cout << "Initial x: " << x << " y: " << y << " z: " << z << " m: " <<
  // m << " theta: " << theta << " phy: " << phy << " Gx: " << Gx << " Gy: " <<
  // Gy << " Gz: " << Gz << "\n";
  Problem problem;
  for (int i = 0; i < int(pSensor_vec.size() / 3); ++i) {
    // problem.AddResidualBlock(
    //     new AutoDiffCostFunction<Cost, 3, 1, 1, 1, 1, 1, 1, 1, 1, 1>(
    //         new Cost(testdata(i, 0), testdata(i, 1), testdata(i, 2),
    //         sPosition(i, 0), sPosition(i, 1), sPosition(i, 2))),
    //     NULL, &x, &y, &z, &m, &theta, &phy, &Gx, &Gy, &Gz);

    problem.AddResidualBlock(
        new AutoDiffCostFunction<Cost_FixedM_1mag, 3, 1, 1, 1, 1, 1, 1, 1, 1>(
            new Cost_FixedM_1mag(readings_vec[i * 3], readings_vec[i * 3 + 1],
                                 readings_vec[i * 3 + 2], pSensor_vec[i * 3],
                                 pSensor_vec[i * 3 + 1], pSensor_vec[i * 3 + 2],
                                 m)),
        NULL, &x, &y, &z, &theta, &phy, &Gx, &Gy, &Gz);
  }
  Solver::Options options;
  // options.max_num_iterations = 1e6;
  options.minimizer_type = ceres::TRUST_REGION;
  options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
  options.minimizer_progress_to_stdout = false;
  options.num_threads = std::thread::hardware_concurrency();
  // options.sparse_linear_algebra_library_type = ceres::EIGEN_SPARSE;
  options.max_num_iterations = 1e5;
  // options.min_relative_decrease = 1e-16;
  // options.max_num_consecutive_invalid_steps = 1e6;
  // options.function_tolerance = 1e-32;
  Solver::Summary summary;
  Solve(options, &problem, &summary);
  // std::cout << summary.FullReport() << "\n";
  // std::cout << "Initial x: " << 0.0 << " y: " << 0.0 << " z: " << 0.0 << " m:
  // " << 0.0 << " theta: " << 0.0 << " phy: " << 0.0 << "\n"; std::cout <<
  // "Final x: " << x << " y: " << y << " z: " << z << " m: " << m << " theta: "
  // << theta << " phy: " << phy << " Gx: " << Gx << " Gy: " << Gy << " Gz: " <<
  // Gz << "\n";

  // set params
  std::vector<double> result_vec = {Gx, Gy, Gz, m, x, y, z, theta, phy};
  return result_vec;
}

std::vector<double> solve_2mag(std::vector<double> readings,
                               std::vector<double> pSensor,
                               std::vector<double> init_param) {
  // std::vector<float> test_vector = { 2,1,3 };
  // Eigen::MatrixXf readings_vec = Eigen::Map<Eigen::Matrix<double, 8, 3>
  // >(readings.data()); Eigen::MatrixXf pSensor_vec =
  // Eigen::Map<Eigen::Matrix<double, 8, 3> >(pSensor.data());
  Eigen::VectorXd readings_vec = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
      readings.data(), readings.size());
  Eigen::VectorXd pSensor_vec = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
      pSensor.data(), pSensor.size());
  // Eigen::MatrixXd readings_vec_1(&readings[0], 8, 3);
  // Eigen::MatrixXd pSensor_vec_1(&pSensor[0], 8, 3);
  // Eigen::Map<Eigen::MatrixXd> readings_vec(readings_vec_1.data(), 3, 8);
  // Eigen::Map<Eigen::MatrixXd> pSensor_vec(pSensor_vec_1.data(), 3, 8);
  // readings_vec = readings_vec.transpose();
  // pSensor_vec = pSensor_vec.transpose();
  // std::cout
  //     << "readings_vec: " << readings_vec << "\n";
  // std::cout << "pSensor_vec: " << pSensor_vec << "\n";

  double Gx = init_param[0];
  double Gy = init_param[1];
  double Gz = init_param[2];
  double m = init_param[3];
  double x0 = init_param[4];
  double y0 = init_param[5];
  double z0 = init_param[6];
  double theta0 = init_param[7];
  double phy0 = init_param[8];
  double x1 = init_param[9];
  double y1 = init_param[10];
  double z1 = init_param[11];
  double theta1 = init_param[12];
  double phy1 = init_param[13];

  // std::cout << "Initial x: " << x << " y: " << y << " z: " << z << " m: " <<
  // m << " theta: " << theta << " phy: " << phy << " Gx: " << Gx << " Gy: " <<
  // Gy << " Gz: " << Gz << "\n";
  Problem problem;
  for (int i = 0; i < int(pSensor_vec.size() / 3); ++i) {
    // problem.AddResidualBlock(
    //     new AutoDiffCostFunction<Cost, 3, 1, 1, 1, 1, 1, 1, 1, 1, 1>(
    //         new Cost(testdata(i, 0), testdata(i, 1), testdata(i, 2),
    //         sPosition(i, 0), sPosition(i, 1), sPosition(i, 2))),
    //     NULL, &x, &y, &z, &m, &theta, &phy, &Gx, &Gy, &Gz);

    problem.AddResidualBlock(
        new AutoDiffCostFunction<Cost_FixedM_2mag, 3, 1, 1, 1, 1, 1, 1, 1, 1, 1,
                                 1, 1, 1, 1>(new Cost_FixedM_2mag(
            readings_vec[i * 3], readings_vec[i * 3 + 1],
            readings_vec[i * 3 + 2], pSensor_vec[i * 3], pSensor_vec[i * 3 + 1],
            pSensor_vec[i * 3 + 2], m)),
        NULL, &Gx, &Gy, &Gz, &x0, &y0, &z0, &theta0, &phy0, &x1, &y1, &z1,
        &theta1, &phy1);
  }
  Solver::Options options;
  // options.max_num_iterations = 1e6;
  options.minimizer_type = ceres::TRUST_REGION;
  options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
  options.minimizer_progress_to_stdout = false;
  options.num_threads = std::thread::hardware_concurrency();
  // options.sparse_linear_algebra_library_type = ceres::EIGEN_SPARSE;
  options.max_num_iterations = 1e5;
  // options.min_relative_decrease = 1e-16;
  // options.max_num_consecutive_invalid_steps = 1e6;
  // options.function_tolerance = 1e-32;
  Solver::Summary summary;
  Solve(options, &problem, &summary);
  // std::cout << summary.FullReport() << "\n";
  // std::cout << "Initial x: " << 0.0 << " y: " << 0.0 << " z: " << 0.0 << " m:
  // " << 0.0 << " theta: " << 0.0 << " phy: " << 0.0 << "\n"; std::cout <<
  // "Final x: " << x << " y: " << y << " z: " << z << " m: " << m << " theta: "
  // << theta << " phy: " << phy << " Gx: " << Gx << " Gy: " << Gy << " Gz: " <<
  // Gz << "\n";

  // set params
  std::vector<double> result_vec = {Gx,     Gy,   Gz, m,  x0, y0,     z0,
                                    theta0, phy0, x1, y1, z1, theta1, phy1};
  return result_vec;
}

std::vector<double> calB(std::vector<double> pSensor,
                         std::vector<double> init_param) {
  Eigen::VectorXd pSensor_vec = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
      pSensor.data(), pSensor.size());

  double Gx = init_param[0];
  double Gy = init_param[1];
  double Gz = init_param[2];
  double m = init_param[3];
  double x = init_param[4];
  double y = init_param[5];
  double z = init_param[6];
  double theta = init_param[7];
  double phy = init_param[8];

  std::vector<double> result, Bi;

  for (int i = 0; i < int(pSensor_vec.size() / 3); ++i) {
    Bi = cal_Bi(pSensor_vec[i * 3], pSensor_vec[i * 3 + 1],
                pSensor_vec[i * 3 + 2], init_param);
    result.insert(result.end(), Bi.begin(), Bi.end());
  }

  // set params
  return result;
}

// wrap C++ function with NumPy array IO
py::array_t<double> py_solve_1mag(
    py::array_t<double, py::array::c_style | py::array::forcecast> readings,
    py::array_t<double, py::array::c_style | py::array::forcecast> pSensor,
    py::array_t<double, py::array::c_style | py::array::forcecast> init_param) {
  // allocate std::vector (to pass to the C++ function)
  std::vector<double> readings_vec(readings.size());
  std::vector<double> pSensor_vec(pSensor.size());
  std::vector<double> init_param_vec(init_param.size());

  std::vector<double> result_vec(init_param.size());
  // copy py::array -> std::vector
  std::memcpy(readings_vec.data(), readings.data(),
              readings.size() * sizeof(double));
  std::memcpy(pSensor_vec.data(), pSensor.data(),
              pSensor.size() * sizeof(double));
  std::memcpy(init_param_vec.data(), init_param.data(),
              init_param.size() * sizeof(double));

  // call pure C++ function
  result_vec = solve_1mag(readings_vec, pSensor_vec, init_param_vec);
  // std::vector<double> result_vec = multiply(array_vec);
  // multiply2(array_vec, result_vec);

  // allocate py::array (to pass the result of the C++ function to Python)
  auto result = py::array_t<double>(init_param.size());
  auto result_buffer = result.request();
  double *result_ptr = (double *)result_buffer.ptr;

  // copy std::vector -> py::array
  std::memcpy(result_ptr, result_vec.data(),
              result_vec.size() * sizeof(double));

  return result;
}

py::array_t<double> py_solve_2mag(
    py::array_t<double, py::array::c_style | py::array::forcecast> readings,
    py::array_t<double, py::array::c_style | py::array::forcecast> pSensor,
    py::array_t<double, py::array::c_style | py::array::forcecast> init_param) {
  // allocate std::vector (to pass to the C++ function)
  std::vector<double> readings_vec(readings.size());
  std::vector<double> pSensor_vec(pSensor.size());
  std::vector<double> init_param_vec(init_param.size());

  std::vector<double> result_vec(init_param.size());
  // copy py::array -> std::vector
  std::memcpy(readings_vec.data(), readings.data(),
              readings.size() * sizeof(double));
  std::memcpy(pSensor_vec.data(), pSensor.data(),
              pSensor.size() * sizeof(double));
  std::memcpy(init_param_vec.data(), init_param.data(),
              init_param.size() * sizeof(double));

  // call pure C++ function
  result_vec = solve_2mag(readings_vec, pSensor_vec, init_param_vec);
  // std::vector<double> result_vec = multiply(array_vec);
  // multiply2(array_vec, result_vec);

  // allocate py::array (to pass the result of the C++ function to Python)
  auto result = py::array_t<double>(init_param.size());
  auto result_buffer = result.request();
  double *result_ptr = (double *)result_buffer.ptr;

  // copy std::vector -> py::array
  std::memcpy(result_ptr, result_vec.data(),
              result_vec.size() * sizeof(double));

  return result;
}

py::array_t<double> py_calB(
    py::array_t<double, py::array::c_style | py::array::forcecast> pSensor,
    py::array_t<double, py::array::c_style | py::array::forcecast> init_param) {
  // allocate std::vector (to pass to the C++ function)
  std::vector<double> pSensor_vec(pSensor.size());
  std::vector<double> init_param_vec(init_param.size());

  std::vector<double> result_vec;
  // copy py::array -> std::vector
  std::memcpy(pSensor_vec.data(), pSensor.data(),
              pSensor.size() * sizeof(double));
  std::memcpy(init_param_vec.data(), init_param.data(),
              init_param.size() * sizeof(double));
  // call pure C++ function

  // std::vector<double> result_vec = multiply(array_vec);
  result_vec = calB(pSensor_vec, init_param_vec);

  // allocate py::array (to pass the result of the C++ function to Python)
  int result_size;
  if (init_param.size() == 9)
    result_size = pSensor_vec.size();
  else
    result_size = (pSensor_vec.size() / 3 + 1) * 3;
  // std::cout << result_size;
  auto result = py::array_t<double>(result_size);
  auto result_buffer = result.request();
  double *result_ptr = (double *)result_buffer.ptr;

  // copy std::vector -> py::array
  std::memcpy(result_ptr, result_vec.data(),
              result_vec.size() * sizeof(double));

  return result;
}

PYBIND11_MODULE(cppsolver, m) {
  m.doc() = R"pbdoc(
        Pybind11 example plugin
        -----------------------

        .. currentmodule:: cppsolver

        .. autosummary::
           :toctree: _generate

           add
           subtract
    )pbdoc";

  m.def(
      "solve_1mag", &py_solve_1mag,
      "solve using the given parameters, sensor readings and sensor positions");

  m.def(
      "solve_2mag", &py_solve_2mag,
      "solve using the given parameters, sensor readings and sensor positions");

  m.def("calB", &py_calB, "Cal B given psensor and params");

#ifdef VERSION_INFO
  m.attr("__version__") = MACRO_STRINGIFY(VERSION_INFO);
#else
  m.attr("__version__") = "dev";
#endif
}