/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2011 - 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, Texas A&M University, 2011
 */

// @sect3{Include files}

// 这个程序的包含文件与之前许多其他程序的包含文件是一样的。唯一的新文件是在介绍中讨论的声明FE_Nothing的文件。hp目录下的文件已经在  step-27  中讨论过了。

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/function.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/sparse_direct.h>
#include <deal.II/lac/affine_constraints.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_tools.h>

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

#include <deal.II/hp/fe_collection.h>
#include <deal.II/hp/fe_values.h>

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

#include <iostream>
#include <fstream>

namespace Step46
{
  using namespace dealii;
// @sect3{The FluidStructureProblem class template}

// 这是主类。如果你想的话，它是 step-8 和 step-22 的组合，因为它的成员变量要么针对全局问题（Triangulation和DoFHandler对象，以及 hp::FECollection 和各种线性代数对象），要么与弹性或斯托克斯子问题有关。然而，该类的一般结构与其他大多数实现静止问题的程序一样。

// 有几个不言自明的辅助函数（cell_is_in_fluid_domain, cell_is_in_solid_domain）（对两个子域的符号名称进行操作，这些名称将被用作属于子域的单元的 material_ids。正如介绍中所解释的那样）和几个函数（make_grid, set_active_fe_indices, assemble_interface_terms），这些函数已经从其他的函数中分离出来，可以在其他的教程程序中找到，我们将在实现它们的时候讨论。

// 最后一组变量 (  viscosity, lambda, eta  ) 描述了用于两个物理模型的材料属性。

  template <int dim>
  class FluidStructureProblem
  {
  public:
    FluidStructureProblem(const unsigned int stokes_degree,
                          const unsigned int elasticity_degree);
    void run();

  private:
    enum
    {
      fluid_domain_id,
      solid_domain_id
    };

    static bool cell_is_in_fluid_domain(
      const typename DoFHandler<dim>::cell_iterator &cell);

    static bool cell_is_in_solid_domain(
      const typename DoFHandler<dim>::cell_iterator &cell);

    void make_grid();
    void set_active_fe_indices();
    void setup_dofs();
    void assemble_system();
    void assemble_interface_term(
      const FEFaceValuesBase<dim> &         elasticity_fe_face_values,
      const FEFaceValuesBase<dim> &         stokes_fe_face_values,
      std::vector<Tensor<1, dim>> &         elasticity_phi,
      std::vector<SymmetricTensor<2, dim>> &stokes_symgrad_phi_u,
      std::vector<double> &                 stokes_phi_p,
      FullMatrix<double> &                  local_interface_matrix) const;
    void solve();
    void output_results(const unsigned int refinement_cycle) const;
    void refine_mesh();

    const unsigned int stokes_degree;
    const unsigned int elasticity_degree;

    Triangulation<dim>    triangulation;
    FESystem<dim>         stokes_fe;
    FESystem<dim>         elasticity_fe;
    hp::FECollection<dim> fe_collection;
    DoFHandler<dim>       dof_handler;

    AffineConstraints<double> constraints;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

    Vector<double> solution;
    Vector<double> system_rhs;

    const double viscosity;
    const double lambda;
    const double mu;
  };
// @sect3{Boundary values and right hand side}

// 下面这个类如其名。速度的边界值分别为2d的
//  $\mathbf u=(0, \sin(\pi x))^T$ 和3d的 $\mathbf u=(0,
//  0, \sin(\pi x)\sin(\pi y))^T$ 。
//  这个问题的其余边界条件都是同质的，在介绍中已经讨论过。右边的强迫项对于流体和固体都是零，所以我们不需要为它设置额外的类。

  template <int dim>
  class StokesBoundaryValues : public Function<dim>
  {
  public:
    StokesBoundaryValues()
      : Function<dim>(dim + 1 + dim)
    {}

    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override;

    virtual void vector_value(const Point<dim> &p,
                              Vector<double> &  value) const override;
  };

  template <int dim>
  double StokesBoundaryValues<dim>::value(const Point<dim> & p,
                                          const unsigned int component) const
  {
    Assert(component < this->n_components,
           ExcIndexRange(component, 0, this->n_components));

    if (component == dim - 1)
      switch (dim)
        {
          case 2:
            return std::sin(numbers::PI * p[0]);
          case 3:
            return std::sin(numbers::PI * p[0]) * std::sin(numbers::PI * p[1]);
          default:
            Assert(false, ExcNotImplemented());
        }

    return 0;
  }

  template <int dim>
  void StokesBoundaryValues<dim>::vector_value(const Point<dim> &p,
                                               Vector<double> &  values) const
  {
    for (unsigned int c = 0; c < this->n_components; ++c)
      values(c) = StokesBoundaryValues<dim>::value(p, c);
  }

//  @sect3{The FluidStructureProblem implementation}
// @sect4{Constructors and helper functions}

// 现在我们来谈谈这个程序的主类的实现。最初的几个函数是构造函数和辅助函数，可以用来确定一个单元格在域的哪个部分。鉴于介绍中对这些主题的讨论，它们的实现是相当明显的。在构造函数中，注意我们必须从斯托克斯和弹性的基本元素中构造 hp::FECollection 对象；使用 hp::FECollection::push_back 函数在这个集合中为它们分配了0和1的位置，我们必须记住这个顺序，并在程序的其余部分一致使用。

  template <int dim>
  FluidStructureProblem<dim>::FluidStructureProblem(
    const unsigned int stokes_degree,
    const unsigned int elasticity_degree)
    : stokes_degree(stokes_degree)
    , elasticity_degree(elasticity_degree)
    , triangulation(Triangulation<dim>::maximum_smoothing)
    , stokes_fe(FE_Q<dim>(stokes_degree + 1),
                dim,
                FE_Q<dim>(stokes_degree),
                1,
                FE_Nothing<dim>(),
                dim)
    , elasticity_fe(FE_Nothing<dim>(),
                    dim,
                    FE_Nothing<dim>(),
                    1,
                    FE_Q<dim>(elasticity_degree),
                    dim)
    , dof_handler(triangulation)
    , viscosity(2)
    , lambda(1)
    , mu(1)
  {
    fe_collection.push_back(stokes_fe);
    fe_collection.push_back(elasticity_fe);
  }

  template <int dim>
  bool FluidStructureProblem<dim>::cell_is_in_fluid_domain(
    const typename DoFHandler<dim>::cell_iterator &cell)
  {
    return (cell->material_id() == fluid_domain_id);
  }

  template <int dim>
  bool FluidStructureProblem<dim>::cell_is_in_solid_domain(
    const typename DoFHandler<dim>::cell_iterator &cell)
  {
    return (cell->material_id() == solid_domain_id);
  }
// @sect4{Meshes and assigning subdomains}

// 接下来的一对函数是处理生成网格，并确保所有表示子域的标志都是正确的。  make_grid  ，正如在介绍中所讨论的，生成一个 $8\times 8$ 的网格（或者一个 $8\times 8\times 8$ 的三维网格）以确保每个粗略的网格单元完全在一个子域内。生成这个网格后，我们在其边界上循环，并在顶部边界设置边界指标为1，这是我们设置非零迪里希特边界条件的唯一地方。在这之后，我们再次在所有单元上循环，设置材料指标—用来表示我们处于域的哪一部分，是流体指标还是固体指标。

  template <int dim>
  void FluidStructureProblem<dim>::make_grid()
  {
    GridGenerator::subdivided_hyper_cube(triangulation, 8, -1, 1);

    for (const auto &cell : triangulation.active_cell_iterators())
      for (const auto &face : cell->face_iterators())
        if (face->at_boundary() && (face->center()[dim - 1] == 1))
          face->set_all_boundary_ids(1);

    for (const auto &cell : dof_handler.active_cell_iterators())
      if (((std::fabs(cell->center()[0]) < 0.25) &&
           (cell->center()[dim - 1] > 0.5)) ||
          ((std::fabs(cell->center()[0]) >= 0.25) &&
           (cell->center()[dim - 1] > -0.5)))
        cell->set_material_id(fluid_domain_id);
      else
        cell->set_material_id(solid_domain_id);
  }

// 这对函数的第二部分决定在每个单元上使用哪个有限元。上面我们设置了每个粗略网格单元的材料指标，正如在介绍中提到的，这个信息在网格细化时将从母单元继承到子单元。

// 换句话说，只要我们细化（或创建）了网格，我们就可以依靠材料指示器来正确描述一个单元所处的域的哪一部分。然后我们利用这一点将单元的活动FE索引设置为该类的 hp::FECollection 成员变量中的相应元素：流体单元为0，固体单元为1。

  template <int dim>
  void FluidStructureProblem<dim>::set_active_fe_indices()
  {
    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        if (cell_is_in_fluid_domain(cell))
          cell->set_active_fe_index(0);
        else if (cell_is_in_solid_domain(cell))
          cell->set_active_fe_index(1);
        else
          Assert(false, ExcNotImplemented());
      }
  }
// @sect4{FluidStructureProblem::setup_dofs}

// 下一步是为线性系统设置数据结构。为此，我们首先要用上面的函数设置活动FE指数，然后分配自由度，再确定线性系统的约束。后者包括像往常一样的悬挂节点约束，但也包括顶部流体边界的不均匀边界值，以及沿固体子域周边的零边界值。

  template <int dim>
  void FluidStructureProblem<dim>::setup_dofs()
  {
    set_active_fe_indices();
    dof_handler.distribute_dofs(fe_collection);

    {
      constraints.clear();
      DoFTools::make_hanging_node_constraints(dof_handler, constraints);

      const FEValuesExtractors::Vector velocities(0);
      VectorTools::interpolate_boundary_values(dof_handler,
                                               1,
                                               StokesBoundaryValues<dim>(),
                                               constraints,
                                               fe_collection.component_mask(
                                                 velocities));

      const FEValuesExtractors::Vector displacements(dim + 1);
      VectorTools::interpolate_boundary_values(
        dof_handler,
        0,
        Functions::ZeroFunction<dim>(dim + 1 + dim),
        constraints,
        fe_collection.component_mask(displacements));
    }

// 不过，我们还需要处理更多的约束条件：我们必须确保在流体和固体的界面上速度为零。下面这段代码已经在介绍中介绍过了。

    {
      std::vector<types::global_dof_index> local_face_dof_indices(
        stokes_fe.n_dofs_per_face());
      for (const auto &cell : dof_handler.active_cell_iterators())
        if (cell_is_in_fluid_domain(cell))
          for (const auto face_no : cell->face_indices())
            if (cell->face(face_no)->at_boundary() == false)
              {
                bool face_is_on_interface = false;

                if ((cell->neighbor(face_no)->has_children() == false) &&
                    (cell_is_in_solid_domain(cell->neighbor(face_no))))
                  face_is_on_interface = true;
                else if (cell->neighbor(face_no)->has_children() == true)
                  {
                    for (unsigned int sf = 0;
                         sf < cell->face(face_no)->n_children();
                         ++sf)
                      if (cell_is_in_solid_domain(
                            cell->neighbor_child_on_subface(face_no, sf)))
                        {
                          face_is_on_interface = true;
                          break;
                        }
                  }

                if (face_is_on_interface)
                  {
                    cell->face(face_no)->get_dof_indices(local_face_dof_indices,
                                                         0);
                    for (unsigned int i = 0; i < local_face_dof_indices.size();
                         ++i)
                      if (stokes_fe.face_system_to_component_index(i).first <
                          dim)
                        constraints.add_line(local_face_dof_indices[i]);
                  }
              }
    }

// 在这一切结束后，我们可以向约束对象声明，我们现在已经准备好了所有的约束，并且该对象可以重建其内部数据结构以提高效率。

    constraints.close();

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

// 在这个函数的其余部分，我们创建了一个在介绍中广泛讨论的稀疏模式，并使用它来初始化矩阵；然后还将向量设置为正确的大小。

    {
      DynamicSparsityPattern dsp(dof_handler.n_dofs(), dof_handler.n_dofs());

      Table<2, DoFTools::Coupling> cell_coupling(fe_collection.n_components(),
                                                 fe_collection.n_components());
      Table<2, DoFTools::Coupling> face_coupling(fe_collection.n_components(),
                                                 fe_collection.n_components());

      for (unsigned int c = 0; c < fe_collection.n_components(); ++c)
        for (unsigned int d = 0; d < fe_collection.n_components(); ++d)
          {
            if (((c < dim + 1) && (d < dim + 1) &&
                 !((c == dim) && (d == dim))) ||
                ((c >= dim + 1) && (d >= dim + 1)))
              cell_coupling[c][d] = DoFTools::always;

            if ((c >= dim + 1) && (d < dim + 1))
              face_coupling[c][d] = DoFTools::always;
          }

      DoFTools::make_flux_sparsity_pattern(dof_handler,
                                           dsp,
                                           cell_coupling,
                                           face_coupling);
      constraints.condense(dsp);
      sparsity_pattern.copy_from(dsp);
    }

    system_matrix.reinit(sparsity_pattern);

    solution.reinit(dof_handler.n_dofs());
    system_rhs.reinit(dof_handler.n_dofs());
  }

//  @sect4{FluidStructureProblem::assemble_system}

// 下面是这个程序的中心函数：组装线性系统的函数。它在开始时有一长段设置辅助函数的内容：从创建正交公式到设置FEValues、FEFaceValues和FESubfaceValues对象，这些都是整合单元项以及界面项所必需的，以应对界面上的单元以相同大小或不同细化程度聚集在一起的情况...

  template <int dim>
  void FluidStructureProblem<dim>::assemble_system()
  {
    system_matrix = 0;
    system_rhs    = 0;

    const QGauss<dim> stokes_quadrature(stokes_degree + 2);
    const QGauss<dim> elasticity_quadrature(elasticity_degree + 2);

    hp::QCollection<dim> q_collection;
    q_collection.push_back(stokes_quadrature);
    q_collection.push_back(elasticity_quadrature);

    hp::FEValues<dim> hp_fe_values(fe_collection,
                                   q_collection,
                                   update_values | update_quadrature_points |
                                     update_JxW_values | update_gradients);

    const QGauss<dim - 1> common_face_quadrature(
      std::max(stokes_degree + 2, elasticity_degree + 2));

    FEFaceValues<dim>    stokes_fe_face_values(stokes_fe,
                                            common_face_quadrature,
                                            update_JxW_values |
                                              update_gradients | update_values);
    FEFaceValues<dim>    elasticity_fe_face_values(elasticity_fe,
                                                common_face_quadrature,
                                                update_normal_vectors |
                                                  update_values);
    FESubfaceValues<dim> stokes_fe_subface_values(stokes_fe,
                                                  common_face_quadrature,
                                                  update_JxW_values |
                                                    update_gradients |
                                                    update_values);
    FESubfaceValues<dim> elasticity_fe_subface_values(elasticity_fe,
                                                      common_face_quadrature,
                                                      update_normal_vectors |
                                                        update_values);

// ...描述局部对全局线性系统贡献所需的对象...

    const unsigned int stokes_dofs_per_cell = stokes_fe.n_dofs_per_cell();
    const unsigned int elasticity_dofs_per_cell =
      elasticity_fe.n_dofs_per_cell();

    FullMatrix<double> local_matrix;
    FullMatrix<double> local_interface_matrix(elasticity_dofs_per_cell,
                                              stokes_dofs_per_cell);
    Vector<double>     local_rhs;

    std::vector<types::global_dof_index> local_dof_indices;
    std::vector<types::global_dof_index> neighbor_dof_indices(
      stokes_dofs_per_cell);

    const Functions::ZeroFunction<dim> right_hand_side(dim + 1);

// ...到变量，允许我们提取形状函数的某些成分并缓存它们的值，而不是在每个正交点重新计算它们。

    const FEValuesExtractors::Vector velocities(0);
    const FEValuesExtractors::Scalar pressure(dim);
    const FEValuesExtractors::Vector displacements(dim + 1);

    std::vector<SymmetricTensor<2, dim>> stokes_symgrad_phi_u(
      stokes_dofs_per_cell);
    std::vector<double> stokes_div_phi_u(stokes_dofs_per_cell);
    std::vector<double> stokes_phi_p(stokes_dofs_per_cell);

    std::vector<Tensor<2, dim>> elasticity_grad_phi(elasticity_dofs_per_cell);
    std::vector<double>         elasticity_div_phi(elasticity_dofs_per_cell);
    std::vector<Tensor<1, dim>> elasticity_phi(elasticity_dofs_per_cell);

// 然后是所有单元格的主循环，和 step-27 一样，初始化当前单元格的 hp::FEValues 对象，提取适合当前单元格的FEValues对象。

    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        hp_fe_values.reinit(cell);

        const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();

        local_matrix.reinit(cell->get_fe().n_dofs_per_cell(),
                            cell->get_fe().n_dofs_per_cell());
        local_rhs.reinit(cell->get_fe().n_dofs_per_cell());

// 做完这些后，我们继续为属于斯托克斯和弹性区域的单元组装单元项。虽然我们原则上可以在一个公式中完成，实际上就是实现了介绍中所说的双线性形式，但我们意识到，我们的有限元空间的选择方式是，在每个单元上，有一组变量（速度和压力，或者位移）总是为零，因此，计算局部积分的更有效的方法是，根据测试我们处于域的哪一部分的 if 条款，只做必要的事情。

// 局部矩阵的实际计算与 step-22 以及 @ref vector_valued 文件模块中给出的弹性方程的计算相同。

        if (cell_is_in_fluid_domain(cell))
          {
            const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
            Assert(dofs_per_cell == stokes_dofs_per_cell, ExcInternalError());

            for (unsigned int q = 0; q < fe_values.n_quadrature_points; ++q)
              {
                for (unsigned int k = 0; k < dofs_per_cell; ++k)
                  {
                    stokes_symgrad_phi_u[k] =
                      fe_values[velocities].symmetric_gradient(k, q);
                    stokes_div_phi_u[k] =
                      fe_values[velocities].divergence(k, q);
                    stokes_phi_p[k] = fe_values[pressure].value(k, q);
                  }

                for (unsigned int i = 0; i < dofs_per_cell; ++i)
                  for (unsigned int j = 0; j < dofs_per_cell; ++j)
                    local_matrix(i, j) +=
                      (2 * viscosity * stokes_symgrad_phi_u[i] *
                         stokes_symgrad_phi_u[j] -
                       stokes_div_phi_u[i] * stokes_phi_p[j] -
                       stokes_phi_p[i] * stokes_div_phi_u[j]) *
                      fe_values.JxW(q);
              }
          }
        else
          {
            const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
            Assert(dofs_per_cell == elasticity_dofs_per_cell,
                   ExcInternalError());

            for (unsigned int q = 0; q < fe_values.n_quadrature_points; ++q)
              {
                for (unsigned int k = 0; k < dofs_per_cell; ++k)
                  {
                    elasticity_grad_phi[k] =
                      fe_values[displacements].gradient(k, q);
                    elasticity_div_phi[k] =
                      fe_values[displacements].divergence(k, q);
                  }

                for (unsigned int i = 0; i < dofs_per_cell; ++i)
                  for (unsigned int j = 0; j < dofs_per_cell; ++j)
                    {
                      local_matrix(i, j) +=
                        (lambda * elasticity_div_phi[i] *
                           elasticity_div_phi[j] +
                         mu * scalar_product(elasticity_grad_phi[i],
                                             elasticity_grad_phi[j]) +
                         mu *
                           scalar_product(elasticity_grad_phi[i],
                                          transpose(elasticity_grad_phi[j]))) *
                        fe_values.JxW(q);
                    }
              }
          }

// 一旦我们得到了单元积分的贡献，我们就把它们复制到全局矩阵中（通过 AffineConstraints::distribute_local_to_global 函数，立即处理约束）。请注意，我们没有向 local_rhs 变量中写入任何东西，尽管我们仍然需要传递它，因为消除非零边界值需要修改局部，因此也需要修改全局的右手值。

        local_dof_indices.resize(cell->get_fe().n_dofs_per_cell());
        cell->get_dof_indices(local_dof_indices);
        constraints.distribute_local_to_global(local_matrix,
                                               local_rhs,
                                               local_dof_indices,
                                               system_matrix,
                                               system_rhs);

// 这个函数更有趣的部分是我们看到关于两个子域之间的界面上的脸部条款。为此，我们首先要确保我们只组装一次，即使在所有单元的所有面的循环中会遇到界面的每一部分两次。我们武断地决定，只有当当前单元是固体子域的一部分，并且因此一个面不在边界上，并且它后面的潜在邻居是流体域的一部分时，我们才会评估界面条款。让我们从这些条件开始。

        if (cell_is_in_solid_domain(cell))
          for (const auto f : cell->face_indices())
            if (cell->face(f)->at_boundary() == false)
              {

// 在这一点上，我们知道当前的单元格是一个候选的整合对象，并且面 f 后面存在一个邻居。现在有三种可能性。

// - 邻居处于同一细化水平，并且没有孩子。

// - 邻居有子女。

// - 邻居比较粗糙。            在所有这三种情况下，我们只对它感兴趣，如果它是流体子域的一部分。因此，让我们从第一种最简单的情况开始：如果邻居处于同一层次，没有子女，并且是一个流体单元，那么这两个单元共享一个边界，这个边界是界面的一部分，我们想沿着这个边界整合界面项。我们所要做的就是用当前面和邻接单元的面初始化两个FEFaceValues对象（注意我们是如何找出邻接单元的哪个面与当前单元接壤的），然后把东西传给评估界面项的函数（这个函数的第三个到第五个参数为它提供了抓取数组）。然后，结果再次被复制到全局矩阵中，使用一个知道本地矩阵的行和列的DoF指数来自不同单元的函数。

                if ((cell->neighbor(f)->level() == cell->level()) &&
                    (cell->neighbor(f)->has_children() == false) &&
                    cell_is_in_fluid_domain(cell->neighbor(f)))
                  {
                    elasticity_fe_face_values.reinit(cell, f);
                    stokes_fe_face_values.reinit(cell->neighbor(f),
                                                 cell->neighbor_of_neighbor(f));

                    assemble_interface_term(elasticity_fe_face_values,
                                            stokes_fe_face_values,
                                            elasticity_phi,
                                            stokes_symgrad_phi_u,
                                            stokes_phi_p,
                                            local_interface_matrix);

                    cell->neighbor(f)->get_dof_indices(neighbor_dof_indices);
                    constraints.distribute_local_to_global(
                      local_interface_matrix,
                      local_dof_indices,
                      neighbor_dof_indices,
                      system_matrix);
                  }

// 第二种情况是，如果邻居还有更多的孩子。在这种情况下，我们必须在邻居的所有子女中进行循环，看他们是否属于流体子域的一部分。如果它们是，那么我们就在共同界面上进行整合，这个界面是邻居的一个面和当前单元的一个子面，要求我们对邻居使用FEFaceValues，对当前单元使用FESubfaceValues。

                else if ((cell->neighbor(f)->level() == cell->level()) &&
                         (cell->neighbor(f)->has_children() == true))
                  {
                    for (unsigned int subface = 0;
                         subface < cell->face(f)->n_children();
                         ++subface)
                      if (cell_is_in_fluid_domain(
                            cell->neighbor_child_on_subface(f, subface)))
                        {
                          elasticity_fe_subface_values.reinit(cell, f, subface);
                          stokes_fe_face_values.reinit(
                            cell->neighbor_child_on_subface(f, subface),
                            cell->neighbor_of_neighbor(f));

                          assemble_interface_term(elasticity_fe_subface_values,
                                                  stokes_fe_face_values,
                                                  elasticity_phi,
                                                  stokes_symgrad_phi_u,
                                                  stokes_phi_p,
                                                  local_interface_matrix);

                          cell->neighbor_child_on_subface(f, subface)
                            ->get_dof_indices(neighbor_dof_indices);
                          constraints.distribute_local_to_global(
                            local_interface_matrix,
                            local_dof_indices,
                            neighbor_dof_indices,
                            system_matrix);
                        }
                  }

// 最后一个选项是，邻居比较粗大。在这种情况下，我们必须为邻居使用一个FESubfaceValues对象，为当前单元使用一个FEFaceValues；其余部分与之前相同。

                else if (cell->neighbor_is_coarser(f) &&
                         cell_is_in_fluid_domain(cell->neighbor(f)))
                  {
                    elasticity_fe_face_values.reinit(cell, f);
                    stokes_fe_subface_values.reinit(
                      cell->neighbor(f),
                      cell->neighbor_of_coarser_neighbor(f).first,
                      cell->neighbor_of_coarser_neighbor(f).second);

                    assemble_interface_term(elasticity_fe_face_values,
                                            stokes_fe_subface_values,
                                            elasticity_phi,
                                            stokes_symgrad_phi_u,
                                            stokes_phi_p,
                                            local_interface_matrix);

                    cell->neighbor(f)->get_dof_indices(neighbor_dof_indices);
                    constraints.distribute_local_to_global(
                      local_interface_matrix,
                      local_dof_indices,
                      neighbor_dof_indices,
                      system_matrix);
                  }
              }
      }
  }

// 在组装全局系统的函数中，我们将计算接口条款传递给我们在此讨论的一个单独的函数。关键是，尽管我们无法预测FEFaceValues和FESubfaceValues对象的组合，但它们都是从FEFaceValuesBase类派生出来的，因此我们不必在意：该函数被简单地调用，有两个这样的对象表示面的两边的正交点上的形状函数值。然后我们做我们一直在做的事情：我们用形状函数的值和它们的导数来填充从头数组，然后循环计算矩阵的所有条目来计算局部积分。我们在这里评估的双线性形式的细节在介绍中给出。

  template <int dim>
  void FluidStructureProblem<dim>::assemble_interface_term(
    const FEFaceValuesBase<dim> &         elasticity_fe_face_values,
    const FEFaceValuesBase<dim> &         stokes_fe_face_values,
    std::vector<Tensor<1, dim>> &         elasticity_phi,
    std::vector<SymmetricTensor<2, dim>> &stokes_symgrad_phi_u,
    std::vector<double> &                 stokes_phi_p,
    FullMatrix<double> &                  local_interface_matrix) const
  {
    Assert(stokes_fe_face_values.n_quadrature_points ==
             elasticity_fe_face_values.n_quadrature_points,
           ExcInternalError());
    const unsigned int n_face_quadrature_points =
      elasticity_fe_face_values.n_quadrature_points;

    const FEValuesExtractors::Vector velocities(0);
    const FEValuesExtractors::Scalar pressure(dim);
    const FEValuesExtractors::Vector displacements(dim + 1);

    local_interface_matrix = 0;
    for (unsigned int q = 0; q < n_face_quadrature_points; ++q)
      {
        const Tensor<1, dim> normal_vector =
          elasticity_fe_face_values.normal_vector(q);

        for (unsigned int k = 0; k < stokes_fe_face_values.dofs_per_cell; ++k)
          {
            stokes_symgrad_phi_u[k] =
              stokes_fe_face_values[velocities].symmetric_gradient(k, q);
            stokes_phi_p[k] = stokes_fe_face_values[pressure].value(k, q);
          }
        for (unsigned int k = 0; k < elasticity_fe_face_values.dofs_per_cell;
             ++k)
          elasticity_phi[k] =
            elasticity_fe_face_values[displacements].value(k, q);

        for (unsigned int i = 0; i < elasticity_fe_face_values.dofs_per_cell;
             ++i)
          for (unsigned int j = 0; j < stokes_fe_face_values.dofs_per_cell; ++j)
            local_interface_matrix(i, j) +=
              -((2 * viscosity * (stokes_symgrad_phi_u[j] * normal_vector) -
                 stokes_phi_p[j] * normal_vector) *
                elasticity_phi[i] * stokes_fe_face_values.JxW(q));
      }
  }
// @sect4{FluidStructureProblem::solve}

// 正如介绍中所讨论的，我们在这里使用了一个相当琐碎的求解器：我们只是将线性系统传递给SparseDirectUMFPACK直接求解器（例如，见 step-29  ）。在求解之后，我们唯一要做的是确保悬挂的节点和边界值约束是正确的。

  template <int dim>
  void FluidStructureProblem<dim>::solve()
  {
    SparseDirectUMFPACK direct_solver;
    direct_solver.initialize(system_matrix);
    direct_solver.vmult(solution, system_rhs);

    constraints.distribute(solution);
  }

//  @sect4{FluidStructureProblem::output_results}

// 生成图形输出在这里相当简单：我们所要做的就是确定解向量的哪些成分属于标量和/或向量（例如，见 step-22 之前的例子），然后把它全部传递给DataOut类。

  template <int dim>
  void FluidStructureProblem<dim>::output_results(
    const unsigned int refinement_cycle) const
  {
    std::vector<std::string> solution_names(dim, "velocity");
    solution_names.emplace_back("pressure");
    for (unsigned int d = 0; d < dim; ++d)
      solution_names.emplace_back("displacement");

    std::vector<DataComponentInterpretation::DataComponentInterpretation>
      data_component_interpretation(
        dim, DataComponentInterpretation::component_is_part_of_vector);
    data_component_interpretation.push_back(
      DataComponentInterpretation::component_is_scalar);
    for (unsigned int d = 0; d < dim; ++d)
      data_component_interpretation.push_back(
        DataComponentInterpretation::component_is_part_of_vector);

    DataOut<dim> data_out;
    data_out.attach_dof_handler(dof_handler);

    data_out.add_data_vector(solution,
                             solution_names,
                             DataOut<dim>::type_dof_data,
                             data_component_interpretation);
    data_out.build_patches();

    std::ofstream output(
      "solution-" + Utilities::int_to_string(refinement_cycle, 2) + ".vtk");
    data_out.write_vtk(output);
  }
// @sect4{FluidStructureProblem::refine_mesh}

// 下一步是细化网格。正如在介绍中所讨论的，这有点棘手，主要是因为流体和固体子域使用的变量具有不同的物理尺寸，因此，误差估计的绝对大小不能直接比较。因此，我们将不得不对它们进行缩放。因此，在函数的顶部，我们首先分别计算不同变量的误差估计值（在流体域中使用速度而不是压力，在固体域中使用位移）。

  template <int dim>
  void FluidStructureProblem<dim>::refine_mesh()
  {
    Vector<float> stokes_estimated_error_per_cell(
      triangulation.n_active_cells());
    Vector<float> elasticity_estimated_error_per_cell(
      triangulation.n_active_cells());

    const QGauss<dim - 1> stokes_face_quadrature(stokes_degree + 2);
    const QGauss<dim - 1> elasticity_face_quadrature(elasticity_degree + 2);

    hp::QCollection<dim - 1> face_q_collection;
    face_q_collection.push_back(stokes_face_quadrature);
    face_q_collection.push_back(elasticity_face_quadrature);

    const FEValuesExtractors::Vector velocities(0);
    KellyErrorEstimator<dim>::estimate(
      dof_handler,
      face_q_collection,
      std::map<types::boundary_id, const Function<dim> *>(),
      solution,
      stokes_estimated_error_per_cell,
      fe_collection.component_mask(velocities));

    const FEValuesExtractors::Vector displacements(dim + 1);
    KellyErrorEstimator<dim>::estimate(
      dof_handler,
      face_q_collection,
      std::map<types::boundary_id, const Function<dim> *>(),
      solution,
      elasticity_estimated_error_per_cell,
      fe_collection.component_mask(displacements));

// 然后，我们通过除以误差估计值的法线对其进行归一化处理，并按照介绍中所讨论的那样，将流体误差指标按4的系数进行缩放。然后将这些结果加在一起，形成一个包含所有单元的误差指标的向量。

    stokes_estimated_error_per_cell *=
      4. / stokes_estimated_error_per_cell.l2_norm();
    elasticity_estimated_error_per_cell *=
      1. / elasticity_estimated_error_per_cell.l2_norm();

    Vector<float> estimated_error_per_cell(triangulation.n_active_cells());

    estimated_error_per_cell += stokes_estimated_error_per_cell;
    estimated_error_per_cell += elasticity_estimated_error_per_cell;

// 在实际细化网格之前，函数的倒数第二部分涉及到我们在介绍中已经提到的启发式方法：由于解是不连续的，KellyErrorEstimator类对位于子域之间边界的单元感到困惑：它认为那里的误差很大，因为梯度的跳跃很大，尽管这完全是预期的，事实上在精确解中也存在这一特征，因此不表明任何数值错误。

// 因此，我们将界面上的所有单元的误差指标设置为零；决定影响哪些单元的条件略显尴尬，因为我们必须考虑到自适应细化网格的可能性，这意味着邻近的单元可能比当前的单元更粗，或者事实上可能被细化一些。这些嵌套条件的结构与我们在 assemble_system 中组装接口条款时遇到的情况基本相同。

    for (const auto &cell : dof_handler.active_cell_iterators())
      for (const auto f : cell->face_indices())
        if (cell_is_in_solid_domain(cell))
          {
            if ((cell->at_boundary(f) == false) &&
                (((cell->neighbor(f)->level() == cell->level()) &&
                  (cell->neighbor(f)->has_children() == false) &&
                  cell_is_in_fluid_domain(cell->neighbor(f))) ||
                 ((cell->neighbor(f)->level() == cell->level()) &&
                  (cell->neighbor(f)->has_children() == true) &&
                  (cell_is_in_fluid_domain(
                    cell->neighbor_child_on_subface(f, 0)))) ||
                 (cell->neighbor_is_coarser(f) &&
                  cell_is_in_fluid_domain(cell->neighbor(f)))))
              estimated_error_per_cell(cell->active_cell_index()) = 0;
          }
        else
          {
            if ((cell->at_boundary(f) == false) &&
                (((cell->neighbor(f)->level() == cell->level()) &&
                  (cell->neighbor(f)->has_children() == false) &&
                  cell_is_in_solid_domain(cell->neighbor(f))) ||
                 ((cell->neighbor(f)->level() == cell->level()) &&
                  (cell->neighbor(f)->has_children() == true) &&
                  (cell_is_in_solid_domain(
                    cell->neighbor_child_on_subface(f, 0)))) ||
                 (cell->neighbor_is_coarser(f) &&
                  cell_is_in_solid_domain(cell->neighbor(f)))))
              estimated_error_per_cell(cell->active_cell_index()) = 0;
          }

    GridRefinement::refine_and_coarsen_fixed_number(triangulation,
                                                    estimated_error_per_cell,
                                                    0.3,
                                                    0.0);
    triangulation.execute_coarsening_and_refinement();
  }

//  @sect4{FluidStructureProblem::run}

// 像往常一样，这是控制整个操作流程的函数。如果你读过教程程序  step-1  到  step-6  ，例如，那么你已经对以下结构相当熟悉。

  template <int dim>
  void FluidStructureProblem<dim>::run()
  {
    make_grid();

    for (unsigned int refinement_cycle = 0; refinement_cycle < 10 - 2 * dim;
         ++refinement_cycle)
      {
        std::cout << "Refinement cycle " << refinement_cycle << std::endl;

        if (refinement_cycle > 0)
          refine_mesh();

        setup_dofs();

        std::cout << "   Assembling..." << std::endl;
        assemble_system();

        std::cout << "   Solving..." << std::endl;
        solve();

        std::cout << "   Writing output..." << std::endl;
        output_results(refinement_cycle);

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

//  @sect4{The main() function}

// 这个，最后的，函数所包含的内容几乎与其他大多数教程程序的内容完全一样。

int main()
{
  try
    {
      using namespace Step46;

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

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

  return 0;
}


#include <armadillo>
#include <iostream>
#include <fstream>
#include <stdio.h>
#include <string>
#include "mcpdft.h"
#include "openrdmConfig.h"

#ifdef WITH_OPENMP // _OPENMP
   #include <omp.h>
#endif

namespace mcpdft {

   MCPDFT::MCPDFT(std::string test_case)  { common_init(test_case); }
   MCPDFT::MCPDFT() {}
   MCPDFT::~MCPDFT() {}

   void MCPDFT::common_init(std::string test_case) {
       print_banner();
       read_grids_from_file(test_case);
       read_orbitals_from_file(test_case);
       read_energies_from_file(test_case);
       if(test_case == "h2_tpbe_sto3g") {
	  is_gga_ = true;
	  read_gradients_from_file(test_case);
       }else{
	  is_gga_ = false;
       }

       read_opdm_from_file(test_case);
       // read_cmat_from_file();
   }

   void MCPDFT::build_rho() {
      build_density_functions();
      if ( is_gga_ ) {
         build_density_gradients();
      }
   }

   void MCPDFT::build_density_functions() {
      int nbfs = get_nbfs();
      size_t npts = get_npts();
      arma::mat phi(get_phi());
      arma::mat D1a(get_D1a());
      arma::mat D1b(get_D1b());
      arma::vec W(get_w());
      arma::vec rhoa(npts, arma::fill::zeros);
      arma::vec rhob(npts, arma::fill::zeros);
      arma::vec rho(npts, arma::fill::zeros);
      double dum_a = 0.0;
      double dum_b = 0.0;
      double dum_tot = 0.0;
      size_t chunk_size = 0;
      int p{0}, mu{0}, nu{0};
      #ifdef WITH_OPENMP
         int nthrds{0};
         printf("\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
         printf("                   *** Warning ***\n");
         printf("   Calculating the density (gradients) using OpenMP");
         printf("\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
         nthrds = omp_get_max_threads();
         nthrds /= 2;
         omp_set_num_threads(nthrds);
      #endif

      #pragma omp parallel default(shared) \
                           private(p, mu, nu)
      {
         #pragma omp for schedule(static) \
                         reduction(+:dum_a, dum_b, dum_tot) \
	                 nowait
            for(p = 0; p < npts; p++) {
               double tempa = 0.0;
               double tempb = 0.0;
               #pragma omp parallel for schedule(static) \
                                        reduction(+:tempa,tempb) \
	                                num_threads(2) \
                                        collapse(2)
                  for(mu = 0; mu < nbfs; mu++) {
                     for(int nu = 0; nu < nbfs; nu++) {
                        tempa += D1a(mu, nu) * phi(p, mu) * phi(p, nu);
                        tempb += D1b(mu, nu) * phi(p, mu) * phi(p, nu);
                     }
                  }
                  rhoa(p) = tempa;
                  rhob(p) = tempb;
                  rho(p) = rhoa(p) + rhob(p);

                  dum_a += rhoa(p) * W(p);
                  dum_b += rhob(p) * W(p);
                  dum_tot += ( rhoa(p) + rhob(p) ) * W(p) ;
            } /* end of omp parallel for loop */
      } /* end of omp parallel region */
      set_rhoa(rhoa);
      set_rhob(rhob);
      set_rho(rho);

      printf("\n");
      printf("  Integrated total density = %20.12lf\n",dum_tot);
      printf("  Integrated alpha density = %20.12lf\n",dum_a);
      printf("  Integrated beta density  = %20.12lf\n",dum_b);
      printf("\n");
   }

   void MCPDFT::build_density_gradients() {
      int nbfs = get_nbfs();
      size_t npts = get_npts();
      arma::mat phi(get_phi());
      arma::mat D1a(get_D1a());
      arma::mat D1b(get_D1b());
      int p{0}, nu{0}, sigma{0};
      arma::mat phi_x(get_phi_x());
      arma::mat phi_y(get_phi_y());
      arma::mat phi_z(get_phi_z());
      arma::vec rho_a_x(npts, arma::fill::zeros);
      arma::vec rho_b_x(npts, arma::fill::zeros);
      arma::vec rho_a_y(npts, arma::fill::zeros);
      arma::vec rho_b_y(npts, arma::fill::zeros);
      arma::vec rho_a_z(npts, arma::fill::zeros);
      arma::vec rho_b_z(npts, arma::fill::zeros);
      arma::vec sigma_aa(npts, arma::fill::zeros);
      arma::vec sigma_ab(npts, arma::fill::zeros);
      arma::vec sigma_bb(npts, arma::fill::zeros);
      #pragma omp parallel default(shared) \
                           private(p, nu, sigma)
      {
         #pragma omp for schedule(static)
             for (int p = 0; p < npts; p++) {
                 double duma_x = 0.0;
                 double dumb_x = 0.0;
                 double duma_y = 0.0;
                 double dumb_y = 0.0;
                 double duma_z = 0.0;
                 double dumb_z = 0.0;
                 #pragma omp parallel for schedule(static) \
                   	                  reduction(+:duma_x, duma_y, duma_z,\
                   		                      dumb_x, dumb_y, dumb_z)\
                                          shared(p, nbfs, \
    				                 sigma_aa, sigma_bb, sigma_ab, \
    				                 rho_a_x, rho_a_y, rho_a_z, \
    				                 rho_b_x, rho_b_y, rho_b_z) \
    			                  num_threads(2) \
                                          collapse(2)
                    for (int sigma = 0; sigma < nbfs; sigma++) {
                        for (int nu = 0; nu < nbfs; nu++) {
                            duma_x += ( phi_x(p, sigma) * phi(p, nu) + phi(p, sigma) * phi_x(p, nu) ) * D1a(sigma, nu);
                            dumb_x += ( phi_x(p, sigma) * phi(p, nu) + phi(p, sigma) * phi_x(p, nu) ) * D1b(sigma, nu);
                            duma_y += ( phi_y(p, sigma) * phi(p, nu) + phi(p, sigma) * phi_y(p, nu) ) * D1a(sigma, nu);
                            dumb_y += ( phi_y(p, sigma) * phi(p, nu) + phi(p, sigma) * phi_y(p, nu) ) * D1b(sigma, nu);
                            duma_z += ( phi_z(p, sigma) * phi(p, nu) + phi(p, sigma) * phi_z(p, nu) ) * D1a(sigma, nu);
                            dumb_z += ( phi_z(p, sigma) * phi(p, nu) + phi(p, sigma) * phi_z(p, nu) ) * D1b(sigma, nu);
                        }
                    }
                    rho_a_x(p) = duma_x;
                    rho_b_x(p) = dumb_x;
                    rho_a_y(p) = duma_y;
                    rho_b_y(p) = dumb_y;
                    rho_a_z(p) = duma_z;
                    rho_b_z(p) = dumb_z;
                    sigma_aa(p) = ( rho_a_x(p) * rho_a_x(p) ) +  ( rho_a_y(p) * rho_a_y(p) ) + ( rho_a_z(p) * rho_a_z(p) );
                    sigma_bb(p) = ( rho_b_x(p) * rho_b_x(p) ) +  ( rho_b_y(p) * rho_b_y(p) ) + ( rho_b_z(p) * rho_b_z(p) );
                    sigma_ab(p) = ( rho_a_x(p) * rho_b_x(p) ) +  ( rho_a_y(p) * rho_b_y(p) ) + ( rho_a_z(p) * rho_b_z(p) );
             }
      }
      set_rhoa_x(rho_a_x);
      set_rhob_x(rho_b_x);
      set_rhoa_y(rho_a_y);
      set_rhob_y(rho_b_y);
      set_rhoa_z(rho_a_z);
      set_rhob_z(rho_b_z);
      set_sigma_aa(sigma_aa);
      set_sigma_ab(sigma_ab);
      set_sigma_bb(sigma_bb);
   }

   void MCPDFT::build_pi(const arma::mat &D2ab) {
      build_ontop_pair_density(D2ab);
      if ( is_gga_ ) {
         build_ontop_pair_density_gradients(D2ab);
      }
   }

   void MCPDFT::build_ontop_pair_density(const arma::mat &D2ab) {
      int nbfs = get_nbfs();
      size_t npts = get_npts();
      arma::vec temp(npts);
      arma::mat phi(get_phi());
      int p{0},
	  mu{0}, nu{0},
	  lambda{0}, sigma{0};
      for (int p = 0; p < npts; p++) {
          double dum = 0.0;
          // pi(r,r) = D(mu,nu; lambda,sigma) * phi(r,mu) * phi(r,nu) * phi(r,lambda) * phi(r,sigma)
          #pragma omp parallel for default(shared) \
          	                       private(mu,nu,lambda,sigma) \
          	                       reduction(+:dum) \
          	                       collapse(4)
             for (int mu = 0; mu < nbfs; mu++) {
                 for (int nu = 0; nu < nbfs; nu++) {
                     for (int lambda = 0; lambda < nbfs; lambda++) {
                         for (int sigma = 0; sigma < nbfs; sigma++) {
                             dum += phi(p, mu) * phi(p, nu) * phi(p, lambda) * phi(p, sigma) * D2ab(nu*nbfs+mu, sigma*nbfs+lambda);
                         }
                     }
                 }
             } /* end of omp parallel for loop */
             temp(p) = dum;
      }
      set_pi(temp);
   }

   void MCPDFT::build_ontop_pair_density_gradients(const arma::mat &D2ab) {
      int nbfs = get_nbfs();
      size_t npts = get_npts();
      int p{0},
	  mu{0}, nu{0},
	  lambda{0}, sigma{0};
      arma::mat phi(get_phi());
      arma::mat phi_x(get_phi_x());
      arma::mat phi_y(get_phi_y());
      arma::mat phi_z(get_phi_z());
      arma::vec pi_x(npts, arma::fill::zeros);
      arma::vec pi_y(npts, arma::fill::zeros);
      arma::vec pi_z(npts, arma::fill::zeros);
      #pragma omp parallel default(shared) \
                           private(p, mu, nu, lambda, sigma)
      {
         #pragma omp for schedule(static)
            for (int p = 0; p < npts; p++) {
                double dum_x = 0.0;
                double dum_y = 0.0;
                double dum_z = 0.0;
                // pi(r) = D(mu,nu; lambda,sigma) * phi(r,mu) * phi(r,nu) * phi(r,lambda) * phi(r,sigma)
                #pragma omp parallel for schedule(static) \
                  	                 reduction(+:dum_x, dum_y, dum_z) \
    		                         num_threads(2) \
                                         collapse(4)
                for (int mu = 0; mu < nbfs; mu++) {
                    for (int nu = 0; nu < nbfs; nu++) {
                        for (int lambda = 0; lambda < nbfs; lambda++) {
                            for (int sigma = 0; sigma < nbfs; sigma++) {
                                dum_x += ( phi_x(p, mu) * phi(p, lambda) * phi(p, sigma) * phi(p, nu) +
                                           phi(p, mu) * phi_x(p, lambda) * phi(p, sigma) * phi(p, nu) +
                                           phi(p, mu) * phi(p, lambda) * phi_x(p, sigma) * phi(p, nu) +
                                           phi(p, mu) * phi(p, lambda) * phi(p, sigma) * phi_x(p, nu) ) * D2ab(nu*nbfs+mu, sigma*nbfs+lambda);

                                dum_y += ( phi_y(p, mu) * phi(p, lambda) * phi(p, sigma) * phi(p, nu) +
                                           phi(p, mu) * phi_y(p, lambda) * phi(p, sigma) * phi(p, nu) +
                                           phi(p, mu) * phi(p, lambda) * phi_y(p, sigma) * phi(p, nu) +
                                           phi(p, mu) * phi(p, lambda) * phi(p, sigma) * phi_y(p, nu) ) * D2ab(nu*nbfs+mu, sigma*nbfs+lambda);

                                dum_z += ( phi_z(p, mu) * phi(p, lambda) * phi(p, sigma) * phi(p, nu) +
                                           phi(p, mu) * phi_z(p, lambda) * phi(p, sigma) * phi(p, nu) +
                                           phi(p, mu) * phi(p, lambda) * phi_z(p, sigma) * phi(p, nu) +
                                           phi(p, mu) * phi(p, lambda) * phi(p, sigma) * phi_z(p, nu) ) * D2ab(nu*nbfs+mu, sigma*nbfs+lambda);
                            }
                        }
                    }
                }
                pi_x(p) = dum_x;
                pi_y(p) = dum_y;
                pi_z(p) = dum_z;
            }
      }
      set_pi_x(pi_x);
      set_pi_z(pi_y);
      set_pi_z(pi_z);
   }

   void MCPDFT::build_R() {
        double tol = 1.0e-20;
        size_t npts = get_npts();
        arma::vec temp(npts);
        arma::vec pi(get_pi());
        arma::vec rho(get_rho());
        #pragma parallel for schedule(static) \
	                     private(p) \
                             shared(npts, pi, rho, temp)
           for (int p = 0; p < npts; p++) {
               temp(p) = 4.0 * pi(p) / ( rho(p) * rho(p) );
           }
        set_R(temp);
   }

   void MCPDFT::translate() {
     translate_density();
     if ( is_gga_ ) {
        translate_density_gradients();
     }
   }

   void MCPDFT::translate_density() {
      double tol = 1.0e-20;
      size_t npts = get_npts();
      arma::vec rho_vec(get_rho());
      arma::vec pi_vec(get_pi());
      arma::vec R_vec(get_R());
      arma::vec W(get_w());
      arma::vec tr_rhoa(npts);
      arma::vec tr_rhob(npts);
      double dum_a = 0.0;
      double dum_b = 0.0;
      double dum_tot = 0.0;
      double rho = 0.0;
      double pi = 0.0;
      double zeta = 0.0;
      double R = 0.0;
      #pragma parallel for schedule(static) \
                           default(shared) \
                           private(p, zeta, pi, rho, R) \
                           reduction(+:dum_a, dum_b, dum_tot)
         for (int p = 0; p < npts; p++) {
             rho = rho_vec(p);
             pi = pi_vec(p);
             zeta = 0.0;
             R = 0.0;
             if ( !(rho < tol) && !(pi < 0.0) ) {
                R = R_vec(p);
                if ( (1.0 - R) > tol ) {
                   zeta = sqrt(1.0 - R);
                }else{
                     zeta = 0.0;
                }
                tr_rhoa(p) = (1.0 + zeta) * (rho/2.0);
                tr_rhob(p) = (1.0 - zeta) * (rho/2.0);
             }else {
                    tr_rhoa(p) = 0.0;
                    tr_rhob(p) = 0.0;
             }
             dum_a += tr_rhoa(p) * W(p);
             dum_b += tr_rhob(p) * W(p);
             dum_tot += ( tr_rhoa(p) + tr_rhob(p) ) * W(p) ;
         }
      set_tr_rhoa(tr_rhoa);
      set_tr_rhob(tr_rhob);

      printf("\n");
      printf("  Integrated translated total density = %20.12lf\n",dum_tot);
      printf("  Integrated translated alpha density = %20.12lf\n",dum_a);
      printf("  Integrated translated beta density  = %20.12lf\n",dum_b);
      printf("\n");
   }

   void MCPDFT::translate_density_gradients() {
     double tol = 1.0e-20;
     size_t npts = get_npts();
     arma::vec rho_vec(get_rho());
     arma::vec pi_vec(get_pi());
     arma::vec R_vec(get_R());
     double rho = 0.0;
     double pi = 0.0;
     double zeta = 0.0;
     double R = 0.0;
     double rho_x = 0.0;
     double rho_y = 0.0;
     double rho_z = 0.0;
     arma::vec rho_a_x(get_rhoa_x());
     arma::vec rho_a_y(get_rhoa_y());
     arma::vec rho_a_z(get_rhoa_z());
     arma::vec rho_b_x(get_rhob_x());
     arma::vec rho_b_y(get_rhob_y());
     arma::vec rho_b_z(get_rhob_z());
     arma::vec tr_rho_a_x(npts,  arma::fill::zeros);
     arma::vec tr_rho_b_x(npts,  arma::fill::zeros);
     arma::vec tr_rho_a_y(npts,  arma::fill::zeros);
     arma::vec tr_rho_b_y(npts,  arma::fill::zeros);
     arma::vec tr_rho_a_z(npts,  arma::fill::zeros);
     arma::vec tr_rho_b_z(npts,  arma::fill::zeros);
     arma::vec tr_sigma_aa(npts, arma::fill::zeros);
     arma::vec tr_sigma_ab(npts, arma::fill::zeros);
     arma::vec tr_sigma_bb(npts, arma::fill::zeros);
     #pragma parallel for schedule(static) \
                          default(shared) \
                          private(p, zeta, pi, rho, R,\
		                  rho_x, rho_y, rho_z)
        for (int p = 0; p < npts; p++) {
            rho = rho_vec(p);
            pi = pi_vec(p);
            rho_x = rho_a_x(p) + rho_b_x(p);
            rho_y = rho_a_y(p) + rho_b_y(p);
            rho_z = rho_a_z(p) + rho_b_z(p);
            zeta = 0.0;
            R = 0.0;
            if ( !(rho < tol) && !(pi < 0.0) ) {
               R = R_vec(p);
               if ( (1.0 - R) > tol )  {
                  zeta = sqrt(1.0 - R);
               }else{
                    zeta = 0.0;
               }
               tr_rho_a_x(p) = (1.0 + zeta) * (rho_x/2.0);
               tr_rho_b_x(p) = (1.0 - zeta) * (rho_x/2.0);

               tr_rho_a_y(p) = (1.0 + zeta) * (rho_y/2.0);
               tr_rho_b_y(p) = (1.0 - zeta) * (rho_y/2.0);

               tr_rho_a_z(p) = (1.0 + zeta) * (rho_z/2.0);
               tr_rho_b_z(p) = (1.0 - zeta) * (rho_z/2.0);
            }else {
                  tr_rho_a_x(p) = 0.0;
                  tr_rho_b_x(p) = 0.0;
                  tr_rho_a_y(p) = 0.0;
                  tr_rho_b_y(p) = 0.0;
                  tr_rho_a_z(p) = 0.0;
                  tr_rho_b_z(p) = 0.0;
            }
            tr_sigma_aa(p) = (tr_rho_a_x(p) * tr_rho_a_x(p)) + (tr_rho_a_y(p) * tr_rho_a_y(p)) + (tr_rho_a_z(p) * tr_rho_a_z(p));
            tr_sigma_ab(p) = (tr_rho_a_x(p) * tr_rho_b_x(p)) + (tr_rho_a_y(p) * tr_rho_b_y(p)) + (tr_rho_a_z(p) * tr_rho_b_z(p));
            tr_sigma_bb(p) = (tr_rho_b_x(p) * tr_rho_b_x(p)) + (tr_rho_b_y(p) * tr_rho_b_y(p)) + (tr_rho_b_z(p) * tr_rho_b_z(p));
        }
     set_tr_sigma_aa(tr_sigma_aa);
     set_tr_sigma_ab(tr_sigma_ab);
     set_tr_sigma_bb(tr_sigma_bb);
   }

   void MCPDFT::fully_translate() {
     fully_translate_density();
     if ( is_gga_ ) {
        fully_translate_density_gradients();
     }
   }

   void MCPDFT::fully_translate_density(){
        double tol = 1.0e-20;
        double const R0 = 0.9;
        double const R1 = 1.15;
        double const A = -475.60656009;
        double const B = -379.47331922;
        double const C = -85.38149682;

        size_t npts = get_npts();
        arma::vec rho_vec(get_rho());
        arma::vec pi_vec(get_pi());
        arma::vec R_vec(get_R());
        arma::vec W(get_w());
        arma::vec tr_rhoa(npts);
        arma::vec tr_rhob(npts);
        arma::vec tr_rho(npts);

        double rho = 0.0;
        double pi = 0.0;
        double zeta = 0.0;
        double R = 0.0;
        double DelR = 0.0;
        double temp_tot = 0.0;
        double temp_a = 0.0;
        double temp_b = 0.0;
        #pragma parallel for schedule(static) \
                             default(shared) \
                             private(p, zeta, pi, rho) \
                             reduction(+:dum_a, dum_b, dum_tot)
        for (int p = 0; p < npts; p++) {
            zeta = 0.0;
            R = 0.0;
            rho = rho_vec(p);
            pi = pi_vec(p);
            DelR = R_vec(p) - R1;
            if ( !(rho < tol) && !(pi < 0.0) ) {
               R = R_vec(p);
               if ( ((1.0 - R) > tol) && ( R < R0 ) ) {
                  zeta = sqrt(1.0 - R);
               }else if( !(R < R0) && !(R > R1) ) {
                       zeta = A * pow(DelR, 5.0) + B * pow(DelR, 4.0) + C * pow(DelR, 3.0);
               }else if( R > R1 ) {
                       zeta = 0.0;
               }
               tr_rhoa(p) = (1.0 + zeta) * (rho/2.0);
               tr_rhob(p) = (1.0 - zeta) * (rho/2.0);
            }else{
                 tr_rhoa(p) = 0.0;
                 tr_rhob(p) = 0.0;
            }
            temp_a += tr_rhoa(p) * W(p);
            temp_b += tr_rhob(p) * W(p);
            temp_tot += ( tr_rhob(p) + tr_rhoa(p) ) * W(p);
        }
        set_tr_rhoa(tr_rhoa);
        set_tr_rhob(tr_rhob);

        printf("\n");
        printf("      Integrated fully translated total density = %20.12lf\n",temp_tot);
        printf("      Integrated fully translated alpha density = %20.12lf\n",temp_a);
        printf("      Integrated fully translated beta density  = %20.12lf\n",temp_b);
        printf("\n");
   }

   void MCPDFT::fully_translate_density_gradients(){
        double tol = 1.0e-20;
        size_t npts = get_npts();

        arma::vec rho_vec(get_rho());
        arma::vec pi_vec(get_pi());
        arma::vec R_vec(get_R());
        arma::vec W(get_w());
        arma::vec tr_rhoa(npts);
        arma::vec tr_rhob(npts);
        arma::vec tr_rho(npts);

        double const R0 = 0.9;
        double const R1 = 1.15;
        double const A = -475.60656009;
        double const B = -379.47331922;
        double const C = -85.38149682;

        arma::vec tr_rho_a_x(npts,  arma::fill::zeros);
        arma::vec tr_rho_b_x(npts,  arma::fill::zeros);
        arma::vec tr_rho_a_y(npts,  arma::fill::zeros);
        arma::vec tr_rho_b_y(npts,  arma::fill::zeros);
        arma::vec tr_rho_a_z(npts,  arma::fill::zeros);
        arma::vec tr_rho_b_z(npts,  arma::fill::zeros);
        arma::vec tr_sigma_aa(npts, arma::fill::zeros);
        arma::vec tr_sigma_ab(npts, arma::fill::zeros);
        arma::vec tr_sigma_bb(npts, arma::fill::zeros);
        double rho_x = 0.0;
        double rho_y = 0.0;
        double rho_z = 0.0;
        double rho = 0.0;
        double pi = 0.0;
        double DelR = 0.0;
        double zeta = 0.0;
        double R = 0.0;
        double temp_tot = 0.0;
        double temp_a = 0.0;
        double temp_b = 0.0;
        #pragma parallel for schedule(static) \
                             default(shared) \
                             private(p, zeta, pi, rho, R, DelR,\
                                     rho_x, rho_y, rho_z)
           for (int p = 0; p < npts; p++) {
               rho_x = rho_a_x_(p) + rho_b_x_(p);
               rho_y = rho_a_y_(p) + rho_b_y_(p);
               rho_z = rho_a_z_(p) + rho_b_z_(p);
               rho = rho_vec(p);
               pi = pi_vec(p);
               DelR = R_vec(p) - R1;
               zeta = 0.0;
               R = 0.0;
               if ( !(rho < tol) && !(pi < 0.0) ) {
                   R = R_vec(p);
                   if ( ((1.0 - R) > tol) && ( R < R0 ) ) {
                      zeta = sqrt(1.0 - R);
                      tr_rho_a_x(p) = (1.0 + zeta) * (rho_x/2.0) + (R * rho_x) / (2.0*zeta) - pi_x_(p) / (rho*zeta);
                      tr_rho_b_x(p) = (1.0 - zeta) * (rho_x/2.0) - (R * rho_x) / (2.0*zeta) + pi_x_(p) / (rho*zeta);
                      tr_rho_a_y(p) = (1.0 + zeta) * (rho_y/2.0) + (R * rho_y) / (2.0*zeta) - pi_y_(p) / (rho*zeta);
                      tr_rho_b_y(p) = (1.0 - zeta) * (rho_y/2.0) - (R * rho_y) / (2.0*zeta) + pi_y_(p) / (rho*zeta);
                      tr_rho_a_z(p) = (1.0 + zeta) * (rho_z/2.0) + (R * rho_z) / (2.0*zeta) - pi_z_(p) / (rho*zeta);
                      tr_rho_b_z(p) = (1.0 - zeta) * (rho_z/2.0) - (R * rho_z) / (2.0*zeta) + pi_z_(p) / (rho*zeta);
                   }else if( !(R < R0) && !(R > R1) ) {
                           zeta = A * pow(DelR, 5.0) + B * pow(DelR, 4.0) + C * pow(DelR, 3.0);

                           tr_rho_a_x(p) = (1.0 + zeta) * (rho_x/2.0)
                                          + (A * pow(DelR, 4.0)) * ( (10.0 * pi_x_(p) / rho) - (5.0 * R * rho_x) )
                                          + (B * pow(DelR, 3.0)) * ( (8.0  * pi_x_(p) / rho) - (4.0 * R * rho_x) )
                                          + (C * pow(DelR, 2.0)) * ( (6.0  * pi_x_(p) / rho) - (3.0 * R * rho_x) );

                           tr_rho_b_x(p) = (1.0 - zeta) * (rho_x/2.0)
                                          + (A * pow(DelR, 4.0)) * (-(10.0 * pi_x_(p) / rho) + (5.0 * R * rho_x) )
                                          + (B * pow(DelR, 3.0)) * (-(8.0  * pi_x_(p) / rho) + (4.0 * R * rho_x) )
                                          + (C * pow(DelR, 2.0)) * (-(6.0  * pi_x_(p) / rho) + (3.0 * R * rho_x) );

                           tr_rho_a_y(p) = (1.0 + zeta) * (rho_y/2.0)
                                          + (A * pow(DelR, 4.0)) * ( (10.0 * pi_y_(p) / rho) - (5.0 * R * rho_y) )
                                          + (B * pow(DelR, 3.0)) * ( (8.0  * pi_y_(p) / rho) - (4.0 * R * rho_y) )
                                          + (C * pow(DelR, 2.0)) * ( (6.0  * pi_y_(p) / rho) - (3.0 * R * rho_y) );

                           tr_rho_b_y(p) = (1.0 - zeta) * (rho_y/2.0)
                                          + (A * pow(DelR, 4.0)) * (-(10.0 * pi_y_(p) / rho) + (5.0 * R * rho_y) )
                                          + (B * pow(DelR, 3.0)) * (-(8.0  * pi_y_(p) / rho) + (4.0 * R * rho_y) )
                                          + (C * pow(DelR, 2.0)) * (-(6.0  * pi_y_(p) / rho) + (3.0 * R * rho_y) );

                           tr_rho_a_z(p) = (1.0 + zeta) * (rho_z/2.0)
                                          + (A * pow(DelR, 4.0)) * ( (10.0 * pi_z_(p) / rho) - (5.0 * R * rho_z) )
                                          + (B * pow(DelR, 3.0)) * ( (8.0  * pi_z_(p) / rho) - (4.0 * R * rho_z) )
                                          + (C * pow(DelR, 2.0)) * ( (6.0  * pi_z_(p) / rho) - (3.0 * R * rho_z) );

                           tr_rho_b_z(p) = (1.0 - zeta) * (rho_z/2.0)
                                           + (A * pow(DelR, 4.0)) * (-(10.0 * pi_z_(p) / rho) + (5.0 * R * rho_z) )
                                           + (B * pow(DelR, 3.0)) * (-(8.0  * pi_z_(p) / rho) + (4.0 * R * rho_z) )
                                           + (C * pow(DelR, 2.0)) * (-(6.0  * pi_z_(p) / rho) + (3.0 * R * rho_z) );
                   }else if( R > R1 ) {
                           zeta = 0.0;
                           tr_rho_a_x(p) = (1.0 + zeta) * (rho_x/2.0);
                           tr_rho_b_x(p) = (1.0 - zeta) * (rho_x/2.0);
                           tr_rho_a_y(p) = (1.0 + zeta) * (rho_y/2.0);
                           tr_rho_b_y(p) = (1.0 - zeta) * (rho_y/2.0);
                           tr_rho_a_z(p) = (1.0 + zeta) * (rho_z/2.0);
                           tr_rho_b_z(p) = (1.0 - zeta) * (rho_z/2.0);
                   }
               }else{
                   tr_rho_a_x(p) = 0.0;
                   tr_rho_b_x(p) = 0.0;
                   tr_rho_a_y(p) = 0.0;
                   tr_rho_b_y(p) = 0.0;
                   tr_rho_a_z(p) = 0.0;
                   tr_rho_b_z(p) = 0.0;
               }
               tr_sigma_aa(p) = (tr_rho_a_x(p) * tr_rho_a_x(p)) + (tr_rho_a_y(p) * tr_rho_a_y(p)) + (tr_rho_a_z(p) * tr_rho_a_z(p));
               tr_sigma_ab(p) = (tr_rho_a_x(p) * tr_rho_b_x(p)) + (tr_rho_a_y(p) * tr_rho_b_y(p)) + (tr_rho_a_z(p) * tr_rho_b_z(p));
               tr_sigma_bb(p) = (tr_rho_b_x(p) * tr_rho_b_x(p)) + (tr_rho_b_y(p) * tr_rho_b_y(p)) + (tr_rho_b_z(p) * tr_rho_b_z(p));
           }
        set_tr_sigma_aa(tr_sigma_aa);
        set_tr_sigma_ab(tr_sigma_ab);
        set_tr_sigma_bb(tr_sigma_bb);
   }

   void MCPDFT::build_opdm() {
      // fetching the number of basis functions
      int nbfs;
      nbfs = get_nbfs();

      // getting the AO->MO transformation matrix C
      arma::mat ca(get_cmat());
      arma::mat cb(get_cmat());

      // building the 1-electron reduced density matrices (1RDMs)
      arma::mat D1a(nbfs, nbfs, arma::fill::zeros);
      arma::mat D1b(nbfs, nbfs, arma::fill::zeros);
      for (int mu = 0; mu < nbfs; mu++) {
          for (int nu = 0; nu < nbfs; nu++) {
              double duma = 0.0;
              double dumb = 0.0;
              for (int i = 0; i < nbfs/2; i++) {
                   duma += ca(mu, i) * ca(nu, i);
                   dumb += cb(mu, i) * cb(nu, i);
              }
              D1a(mu, nu) = duma;
              D1b(mu, nu) = dumb;
         }
      }
      // D1a(0,0) = 1.0;
      // D1b(0,0) = 1.0;
      D1a.print("D1a = ");
      D1b.print("D1b = ");
      set_D1a(D1a);
      set_D1b(D1b);
   }

   void MCPDFT::build_tpdm() {
      // fetching the number of basis functions
      int nbfs  = get_nbfs();
      int nbfs2 = nbfs * nbfs;

      arma::mat D1a(get_D1a());
      arma::mat D1b(get_D1b());

      arma::mat D2ab(nbfs2, nbfs2, arma::fill::zeros);
      D2ab = arma::kron(D1a,D1b);
      // D2ab.print("D2ab = ");
      set_D2ab(D2ab);
   }

   bool MCPDFT::is_gga() const { return is_gga_; }
   size_t MCPDFT::get_npts() const { return npts_; }
   int    MCPDFT::get_nbfs() const { return nbfs_; }
   arma::vec MCPDFT::get_w() const { return w_; }
   arma::vec MCPDFT::get_x() const { return x_; }
   arma::vec MCPDFT::get_y() const { return y_; }
   arma::vec MCPDFT::get_z() const { return z_; }
   arma::mat MCPDFT::get_phi() const { return phi_; }
   arma::mat MCPDFT::get_phi_x() const { return phi_x_; }
   arma::mat MCPDFT::get_phi_y() const { return phi_y_; }
   arma::mat MCPDFT::get_phi_z() const { return phi_z_; }
   double MCPDFT::get_eref() const { return eref_; }
   double MCPDFT::get_eclass()  const { return eclass_; }
   arma::mat MCPDFT::get_cmat() const { return cmat_; }
   arma::mat MCPDFT::get_D1a()  const { return D1a_ ; }
   arma::mat MCPDFT::get_D1b()  const { return D1b_ ; }
   arma::mat MCPDFT::get_D2ab()  const { return D2ab_ ; }
   arma::vec MCPDFT::get_rhoa() const { return rho_a_; }
   arma::vec MCPDFT::get_rhoa_x() const { return rho_a_x_; }
   arma::vec MCPDFT::get_rhoa_y() const { return rho_a_y_; }
   arma::vec MCPDFT::get_rhoa_z() const { return rho_a_z_; }
   arma::vec MCPDFT::get_rhob() const { return rho_b_; }
   arma::vec MCPDFT::get_rhob_x() const { return rho_b_x_; }
   arma::vec MCPDFT::get_rhob_y() const { return rho_b_y_; }
   arma::vec MCPDFT::get_rhob_z() const { return rho_b_z_; }
   arma::vec MCPDFT::get_rho() const { return rho_; }
   arma::vec MCPDFT::get_tr_rhoa() const { return tr_rho_a_; }
   arma::vec MCPDFT::get_tr_rhob() const { return tr_rho_b_; }
   arma::vec MCPDFT::get_tr_rho() const { return tr_rho_; }
   arma::vec MCPDFT::get_pi() const { return pi_; }
   arma::vec MCPDFT::get_R() const { return R_; }
   arma::vec MCPDFT::get_sigma_aa() const { return sigma_aa_; }
   arma::vec MCPDFT::get_sigma_ab() const { return sigma_ab_; }
   arma::vec MCPDFT::get_sigma_bb() const { return sigma_bb_; }
   arma::vec MCPDFT::get_tr_sigma_aa() const { return tr_sigma_aa_; }
   arma::vec MCPDFT::get_tr_sigma_ab() const { return tr_sigma_ab_; }
   arma::vec MCPDFT::get_tr_sigma_bb() const { return tr_sigma_bb_; }

   void MCPDFT::set_npts(const size_t npts) { npts_ = npts; }
   void MCPDFT::set_nbfs(const int nbfs)    { nbfs_ = nbfs; }
   void MCPDFT::set_w(const arma::vec &w) { w_ = w; }
   void MCPDFT::set_x(const arma::vec &x) { x_ = x; }
   void MCPDFT::set_y(const arma::vec &y) { y_ = y; }
   void MCPDFT::set_z(const arma::vec &z) { z_ = z; }
   void MCPDFT::set_phi(const arma::mat &phi) { phi_ = phi; }
   void MCPDFT::set_phi_x(const arma::mat &phi_x) { phi_x_ = phi_x; }
   void MCPDFT::set_phi_y(const arma::mat &phi_y) { phi_y_ = phi_y; }
   void MCPDFT::set_phi_z(const arma::mat &phi_z) { phi_z_ = phi_z; }
   void MCPDFT::set_eref(const double eref) { eref_ = eref; }
   void MCPDFT::set_eclass(const double eclass) { eclass_ = eclass; }
   void MCPDFT::set_cmat(const arma::mat &cmat) { cmat_ = cmat; }
   void MCPDFT::set_D1a(const arma::mat &D1a) { D1a_ = D1a; }
   void MCPDFT::set_D1b(const arma::mat &D1b) { D1b_ = D1b; }
   void MCPDFT::set_D2ab(const arma::mat &D2ab) { D2ab_ = D2ab; }
   void MCPDFT::set_rhoa(const arma::vec &rhoa) { rho_a_ = rhoa; }
   void MCPDFT::set_rhoa_x(const arma::vec &rhoa_x) { rho_a_x_ = rhoa_x; }
   void MCPDFT::set_rhoa_y(const arma::vec &rhoa_y) { rho_a_y_ = rhoa_y; }
   void MCPDFT::set_rhoa_z(const arma::vec &rhoa_z) { rho_a_z_ = rhoa_z; }
   void MCPDFT::set_rhob(const arma::vec &rhob) { rho_b_ = rhob; }
   void MCPDFT::set_rhob_x(const arma::vec &rhob_x) { rho_b_x_ = rhob_x; }
   void MCPDFT::set_rhob_y(const arma::vec &rhob_y) { rho_b_y_ = rhob_y; }
   void MCPDFT::set_rhob_z(const arma::vec &rhob_z) { rho_b_z_ = rhob_z; }
   void MCPDFT::set_rho(const arma::vec &rho) { rho_ = rho; }
   void MCPDFT::set_tr_rhoa(const arma::vec &tr_rhoa) { tr_rho_a_ = tr_rhoa; }
   void MCPDFT::set_tr_rhoa_x(const arma::vec &tr_rhoa_x) { tr_rho_a_x_ = tr_rhoa_x; }
   void MCPDFT::set_tr_rhoa_y(const arma::vec &tr_rhoa_y) { tr_rho_a_y_ = tr_rhoa_y; }
   void MCPDFT::set_tr_rhoa_z(const arma::vec &tr_rhoa_z) { tr_rho_a_z_ = tr_rhoa_z; }
   void MCPDFT::set_tr_rhob(const arma::vec &tr_rhob) { tr_rho_b_ = tr_rhob; }
   void MCPDFT::set_tr_rhob_x(const arma::vec &tr_rhob_x) { tr_rho_b_x_ = tr_rhob_x; }
   void MCPDFT::set_tr_rhob_y(const arma::vec &tr_rhob_y) { tr_rho_b_y_ = tr_rhob_y; }
   void MCPDFT::set_tr_rhob_z(const arma::vec &tr_rhob_z) { tr_rho_b_z_ = tr_rhob_z; }
   void MCPDFT::set_tr_rho(const arma::vec &tr_rho) { tr_rho_ = tr_rho; }
   void MCPDFT::set_pi(const arma::vec &pi) { pi_ = pi; }
   void MCPDFT::set_pi_x(const arma::vec &pi_x) { pi_x_ = pi_x; }
   void MCPDFT::set_pi_y(const arma::vec &pi_y) { pi_y_ = pi_y; }
   void MCPDFT::set_pi_z(const arma::vec &pi_z) { pi_z_ = pi_z; }
   void MCPDFT::set_R(const arma::vec &R) { R_ = R; }
   void MCPDFT::set_sigma_aa(const arma::vec &sigma_aa) { sigma_aa_ = sigma_aa; }
   void MCPDFT::set_sigma_ab(const arma::vec &sigma_ab) { sigma_ab_ = sigma_ab; }
   void MCPDFT::set_sigma_bb(const arma::vec &sigma_bb) { sigma_bb_ = sigma_bb; }
   void MCPDFT::set_tr_sigma_aa(const arma::vec &tr_sigma_aa) { tr_sigma_aa_ = tr_sigma_aa; }
   void MCPDFT::set_tr_sigma_ab(const arma::vec &tr_sigma_ab) { tr_sigma_ab_ = tr_sigma_ab; }
   void MCPDFT::set_tr_sigma_bb(const arma::vec &tr_sigma_bb) { tr_sigma_bb_ = tr_sigma_bb; }

   void MCPDFT::print_banner() const {
      printf("\n******************************************************************\n");
      printf("*                                                                *\n");
      printf("*                           OpenRDM:                             *\n");
      printf("*                                                                *\n");
      printf("*                 An open-source library for                     *\n");
      printf("*    reduced-density matrix-based analysis and computation       *\n");
      printf("*                                                                *\n");
      printf("*                     Mohammad Mostafanejad                      *\n");
      printf("*                   Florida State University                     *\n");
      printf("*                                                                *\n");
      printf("******************************************************************\n");

      printf("\n           Please cite the following article(s):\n\n");

      printf("    # M. Mostafanejad and A. E. DePrince III\n");
      printf("      J. Chem. Theory Comput. 15, 290-302 (2019).\n");
   }
}


#include "extent.hpp"

#include <algorithm>
#include <boost/algorithm/clamp.hpp>
#include <boost/math/constants/constants.hpp>
#include <cmath>
#include <utility>
#include "../common/geom.hpp"
#include "../common/helpers/eigen_helpers.hpp"

const double PI = boost::math::constants::pi<double>();

namespace ear {

  /** @brief Normalise position or return {0,1,0}.
   *
   * @param position  Position to normalise.
   *
   * @returns normalised position
   */
  Eigen::Vector3d safeNormPosition(Eigen::Vector3d position) {
    double norm = position.norm();
    if (norm < 1e-10) {
      return Eigen::Vector3d{0.0, 1.0, 0.0};
    } else {
      return position / norm;
    }
  }

  double extentMod(double extent, double distance) {
    double minSize = 0.2;
    double size = interp(extent, Eigen::Vector2d(0.0, 360.0),
                         Eigen::Vector2d(minSize, 1.0));
    double extent1 = 4.0 * degrees(atan2(size, 1.0));
    return interp(4.0 * degrees(atan2(size, distance)),
                  Eigen::Vector3d(0.0, extent1, 360.0),
                  Eigen::Vector3d(0.0, extent, 360.0));
  }

  Eigen::Matrix3d calcBasis(Eigen::Vector3d position) {
    position = safeNormPosition(position);
    double az = azimuth(position);
    double el = elevation(position);

    // points near the poles have indeterminate azimuth; assume 0
    if (std::abs(el) > (90.0 - 1e-5)) {
      az = 0.0;
    }
    return localCoordinateSystem(az, el);
  }

  Eigen::Vector3d cartOnBasis(Eigen::Matrix3d basis, double azimuth,
                              double elevation) {
    Eigen::RowVector3d cartPosRel{sin(azimuth) * cos(elevation),
                                  cos(azimuth) * cos(elevation),
                                  sin(elevation)};
    return cartPosRel * basis;
  }

  std::pair<double, double> azimuthElevationOnBasis(
      Eigen::Matrix3d basis, Eigen::RowVector3d position) {
    // project onto each basis, and clip components to keep asin happy
    Eigen::Vector3d components =
        (position * basis.transpose()).cwiseMin(1.0).cwiseMax(-1.0);

    double azimuth = atan2(components(0), components(1));
    double elevation = asin(components(2));

    return std::make_pair(azimuth, elevation);
  }

  WeightingFunction::WeightingFunction(Eigen::Vector3d position, double width,
                                       double height) {
    _width = radians(width) / 2;
    _height = radians(height) / 2;

    // basis vectors to rotate the vsource positions towards position
    Eigen::Matrix3d basises = calcBasis(position);

    _circleRadius = std::min(_width, _height);

    // Flip the width and the height such that it is always wider than it is
    // high from here in.
    if (_height > _width) {
      std::swap(_height, _width);
      _flippedBasis = basises.colwise().reverse();
    } else {
      _flippedBasis = basises;
    }

    // modify the width to make it meet at the back.
    double widthFull = PI + _height;
    // interpolate to this from a width of pi/2 to pi
    double widthMod = interp(_width, Eigen::Vector3d{0.0, PI / 2.0, PI},
                             Eigen::Vector3d{0.0, PI / 2.0, widthFull});
    // apply this fully for a height of less than pi/4; tail off until pi/2
    _width = interp(_height, Eigen::Vector4d{0, PI / 4.0, PI / 2.0, PI},  //
                    Eigen::Vector4d{widthMod, widthMod, _width, _width});  //

    // angle of the circle centres from the source position; width is to the
    // end of the rectangle.
    _circlePos = _width - _circleRadius;

    // Cartesian circle centres
    _circlePositions << cartOnBasis(_flippedBasis, -_circlePos, 0.0),
        cartOnBasis(_flippedBasis, _circlePos, 0.0);
  }

  double WeightingFunction::operator()(Eigen::Vector3d position) const {
    // Flipped azimuths and elevations; the straight edges are always along
    // azimuth lines.
    double azimuth, elevation;
    std::tie(azimuth, elevation) =
        azimuthElevationOnBasis(_flippedBasis, position);

    // The distance is the angle away from the defined shape; 0 or negative is
    // inside.
    double distance = 0.0;

    // for the straight lines
    if (std::abs(azimuth) <= _circlePos) {
      distance = std::abs(elevation) - _circleRadius;
    } else {
      // distance from the closest circle centre
      size_t nearest_circle = azimuth < 0 ? 0 : 1;
      double angle =
          position.transpose() * _circlePositions.col(nearest_circle);
      double circleDistance = acos(boost::algorithm::clamp(angle, -1.0, 1.0));
      distance = circleDistance - _circleRadius;
    }
    // fade the weight from one to zero over fadeWidth
    return interp(distance, Eigen::Vector2d{0.0, radians(_fadeWidth)},
                  Eigen::Vector2d{1.0, 0.0});
  }

  SpreadingPanner::SpreadingPanner(std::shared_ptr<PointSourcePanner> psp,
                                   int nRows)
      : _psp(psp), _nRows(nRows) {
    _panningPositions = _generatePanningPositionsEven();
    _panningPositionsResults = _generatePanningPositionsResults();
  }

  Eigen::VectorXd SpreadingPanner::panningValuesForWeight(
      const WeightingFunction& weightFunc) {
    Eigen::VectorXd weights(_panningPositions.rows());
    for (int i = 0; i < _panningPositions.rows(); ++i) {
      weights(i) = weightFunc(_panningPositions.row(i));
    }
    Eigen::VectorXd totalPv = weights.transpose() * _panningPositionsResults;
    return totalPv / totalPv.norm();
  }

  Eigen::MatrixXd SpreadingPanner::_generatePanningPositionsEven() {
    Eigen::VectorXd elevations =
        Eigen::VectorXd::LinSpaced(_nRows, -90.0, 90.0);
    Eigen::MatrixXd positions(0, 3);

    for (double el : elevations) {
      double radius = cos(radians(el));
      double perimiter = 2 * PI * radius;
      double perimiter_centre = 2 * PI;

      int nPoints = static_cast<int>(
          std::round((perimiter / perimiter_centre) * 2 * (_nRows - 1)));
      if (nPoints == 0) {
        nPoints = 1;
      }
      Eigen::VectorXd azimuths =
          Eigen::VectorXd::LinSpaced(nPoints + 1, 0.0, 360.0);
      for (int i = 0; i < azimuths.size() - 1; ++i) {
        double az = azimuths(i);
        positions.conservativeResize(positions.rows() + 1, Eigen::NoChange);
        positions.row(positions.rows() - 1) = cart(az, el, 1.0);
      }
    }
    return positions;
  }

  Eigen::MatrixXd SpreadingPanner::_generatePanningPositionsResults() {
    Eigen::MatrixXd results(_panningPositions.rows(),
                            _psp->numberOfOutputChannels());
    for (int i = 0; i < _panningPositions.rows(); ++i) {
      results.row(i) = _psp->handle(_panningPositions.row(i)).get();
    }
    return results;
  }

  PolarExtentPanner::PolarExtentPanner(std::shared_ptr<PointSourcePanner> psp)
      : _psp(psp), _spreadingPanner(SpreadingPanner(psp, _nRows)){};

  Eigen::VectorXd PolarExtentPanner::calcPvSpread(Eigen::Vector3d position,
                                                  double width, double height) {
    // When calculating the spread panning values the width and height are
    // set to at least fade_width. For sizes where any of the dimensions is
    // less than this, interpolate linearly between the point and spread
    // panning values.
    double ammount_spread =
        interp(std::max(width, height), Eigen::Vector2d(0.0, _fadeWidth),
               Eigen::Vector2d(0.0, 1.0));
    double ammountPoint = 1.0 - ammount_spread;
    Eigen::ArrayXd pv = Eigen::ArrayXd::Zero(_psp->numberOfOutputChannels());
    if (ammountPoint > 1e-10) {
      pv += ammountPoint * _psp->handle(position).get().array().square();
    }
    if (ammount_spread > 1e-10) {
      // minimum width and height as above
      width = std::max(width, _fadeWidth / 2.0);
      height = std::max(height, _fadeWidth / 2.0);

      WeightingFunction weightingFunction(position, width, height);
      Eigen::VectorXd panning_values =
          _spreadingPanner.panningValuesForWeight(weightingFunction);
      pv += ammount_spread * panning_values.array().square();
    }
    return pv.sqrt().matrix();
  }

  Eigen::VectorXd PolarExtentPanner::handle(Eigen::Vector3d position,
                                            double width, double height,
                                            double depth) {
    double distance = position.norm();

    if (depth != 0.0) {
      double distanceMin = distance - depth / 2.0;
      double distanceMax = distance + depth / 2.0;
      distanceMin = (distanceMin < 0) ? 0.0 : distanceMin;
      distanceMax = (distanceMax < 0) ? 0.0 : distanceMax;
      Eigen::VectorXd pvsMin =
          calcPvSpread(position, extentMod(width, distanceMin),
                       extentMod(height, distanceMin));
      Eigen::VectorXd pvsMax =
          calcPvSpread(position, extentMod(width, distanceMax),
                       extentMod(height, distanceMax));
      return ((pvsMin.array().square() + pvsMax.array().square()) / 2.0).sqrt();
    } else {
      Eigen::VectorXd pvs = calcPvSpread(position, extentMod(width, distance),
                                         extentMod(height, distance));
      return pvs;
    }
  }

}  // namespace ear


#ifndef STAN_MATH_PRIM_SCAL_FUN_BESSEL_SECOND_KIND_HPP
#define STAN_MATH_PRIM_SCAL_FUN_BESSEL_SECOND_KIND_HPP

#include <boost/math/special_functions/bessel.hpp>

namespace stan {
namespace math {

/**
 *
   \f[
   \mbox{bessel\_second\_kind}(v, x) =
   \begin{cases}
     \textrm{error} & \mbox{if } x \leq 0 \\
     Y_v(x) & \mbox{if } x > 0 \\[6pt]
     \textrm{NaN} & \mbox{if } x = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{bessel\_second\_kind}(v, x)}{\partial x} =
   \begin{cases}
     \textrm{error} & \mbox{if } x \leq 0 \\
     \frac{\partial\, Y_v(x)}{\partial x} & \mbox{if } x > 0 \\[6pt]
     \textrm{NaN} & \mbox{if } x = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   Y_v(x)=\frac{J_v(x)\cos(v\pi)-J_{-v}(x)}{\sin(v\pi)}
   \f]

   \f[
   \frac{\partial \, Y_v(x)}{\partial x} = \frac{v}{x}Y_v(x)-Y_{v+1}(x)
   \f]
 *
 */
template <typename T2>
inline T2 bessel_second_kind(int v, const T2 z) {
  return boost::math::cyl_neumann(v, z);
}

}  // namespace math
}  // namespace stan

#endif


/*! \file solveeom.cpp
    \brief 単振り子に対して運動方程式を解くクラスの実装

    Copyright ©  2016 @dc1394 All Rights Reserved.
    (but this is originally adapted by Freddie Witherden for doublependulum.cpp from https://freddie.witherden.org/tools/doublependulum/ )
    This software is released under the BSD 2-Clause License.
*/
#include "solveeom.h"
#include <cmath>                                // for std::sin, std::cos
#include <fstream>                              // for std::ofstream
#include <boost/assert.hpp>                     // for BOOST_ASSERT
#include <boost/format.hpp>                     // for boost::format
#include <boost/math/constants/constants.hpp>   // for boost::math::constants::pi

namespace solveeom {
    // #region コンストラクタ

    SolveEOM::SolveEOM(float l, float m, float theta1_0, float theta2_0) :
        Theta1([this] { return static_cast<float>(x_[0]); }, [this](auto theta) { return x_[0] = theta; }),
        Theta2([this] { return static_cast<float>(x_[2]); }, [this](auto theta) { return x_[2] = theta; }),
        V1([this] { return static_cast<float>(x_[1]); }, [this](auto v) { return x_[1] = v; }),
        V2([this] { return static_cast<float>(x_[3]); }, [this](auto v) { return x_[3] = v; }),
        l_(l),
        m_(m),
        stepper_(SolveEOM::EPS, SolveEOM::EPS)
    {

        x_ = { theta1_0, 0.0, theta2_0, 0.0 };
    }

    // #endregion コンストラクタ

    // #region publicメンバ関数

    float SolveEOM::kinetic_energy() const
    {
        return static_cast<float>(
            m_ * l_ * l_ * (sqr(x_[1]) + 0.5 * sqr(x_[3])) +
            m_ * l_ * l_ * x_[1] * x_[3] * std::cos(x_[0] - x_[2]));
    }

    void SolveEOM::operator()(float dt, float * theta1, float * theta2)
    {
        boost::numeric::odeint::integrate_adaptive(
            stepper_,
            getEOM(),
            x_,
            0.0,
            static_cast<double>(dt),
            SolveEOM::DX);

        *theta1 = static_cast<float>(x_[0]);
        *theta2 = static_cast<float>(x_[2]);
    }

    void SolveEOM::operator()(double dt, std::string const & filename, double t)
    {
        std::ofstream result(filename);

        boost::numeric::odeint::integrate_const(
            stepper_,
            getEOM(),
            x_,
            0.0,
            t,
            dt,
            [&result, this](auto const & x, auto const t)
        {
			result << boost::format("%.3f, %.15f, %.15f, %.15f\n") % t % x[0] % x[2] % total_energy();
        });
    }

    float SolveEOM::potential_energy() const
    {
        return static_cast<float>(m_ * SolveEOM::g * l_ * (3.0 - 2.0 * std::cos(x_[0]) - std::cos(x_[2])));
    }

    // #endregion publicメンバ関数

    // #region privateメンバ関数

    std::function<void(SolveEOM::state_type const &, SolveEOM::state_type &, double const)> SolveEOM::getEOM() const
    {
        auto const eom = [this](state_type const & x, state_type & dxdt, double const)
        {
            // Delta is θ2 - θ1
            auto const delta = x[Num_eqns::THETA_2] - x[Num_eqns::THETA_1];

            // `Big-M' is the total mass of the system, m1 + m2;
            auto const M = 2.0 * m_;

            // Denominator expression for ω1
            auto den = M * l_ - m_ * l_ * std::cos(delta) * std::cos(delta);

            // dθ/dt = ω, by definition
            dxdt[Num_eqns::THETA_1] = x[Num_eqns::OMEGA_1];

            // Compute ω1
            dxdt[OMEGA_1] = (m_ * l_ * x[Num_eqns::OMEGA_1] * x[Num_eqns::OMEGA_1] * std::sin(delta) * std::cos(delta)
                + m_ * g * std::sin(x[Num_eqns::THETA_2]) * std::cos(delta)
                + m_ * l_ * x[Num_eqns::OMEGA_2] * x[Num_eqns::OMEGA_2] * std::sin(delta)
                - M * g * std::sin(x[THETA_1])) / den;

            // Again, dθ/dt = ω for θ2 as well
            dxdt[THETA_2] = x[OMEGA_2];

            // Multiply den by the length ratio of the two bobs
            den *= l_ / l_;

            // Compute ω2
            dxdt[Num_eqns::OMEGA_2] = (-m_ * l_ * x[Num_eqns::OMEGA_2] * x[Num_eqns::OMEGA_2] * std::sin(delta) * std::cos(delta)
                + M * g * std::sin(x[Num_eqns::THETA_1]) * std::cos(delta)
                - M * l_ * x[OMEGA_1] * x[Num_eqns::OMEGA_1] * std::sin(delta)
                - M * g * std::sin(x[Num_eqns::THETA_2])) / den;
        };

        return eom;
    }

	double SolveEOM::total_energy() const
	{
		auto const kinetic = m_ * l_ * l_ * (sqr(x_[1]) + 0.5 * sqr(x_[3])) + m_ * l_ * l_ * x_[1] * x_[3] * std::cos(x_[0] - x_[2]);
		auto const potential = m_ * SolveEOM::g * l_ * (3.0 - 2.0 * std::cos(x_[0]) - std::cos(x_[2]));

		return kinetic + potential;
	}

    // #endregion privateメンバ関数
}


/*! \file foelement.cpp
    \brief Bogoliubov-de Gennes方程式を解くクラスの実装
    Copyright ©  2016 @dc1394 All Rights Reserved.
    (but this is originally adapted by cometscome for Chev.py from https://github.com/cometscome/ChebyshevPolynomialBdG )
    This software is released under the BSD 2-Clause License.
*/

#include "chev.h"
#include <cmath>                // for std::acos, for std::sin
#include <iomanip>              // for std::setprecision
#include <iostream>             // for std::cout
#include <vector>               // for std::vector
#include <Eigen/EigenValues>    // for Eigen::SelfAdjointEigenSolver

namespace chebyshevpolynomialbdg {
    // #region コンストラクタ

    Chev::Chev()
        :   vec_ai_(Chev::NC),
            vec_delta_(Chev::LN_2, Chev::LN_2)
    {
        std::cout.setf(std::ios::fixed, std::ios::floatfield);
    }

    // #endregion コンストラクタ

    // #region publicメンバ関数

    void Chev::iteration(bool full)
    {
        std::cout << std::setprecision(15);

        init_delta();
        calc_A();
        Eigen::SparseMatrix<double> vec_delta_old(vec_delta_);

        for (auto ite = 0; ite < ITERMAX; ite++) {
            if (full) {
                calc_meanfields<true>();
            }
            else {
                calc_meanfields<false>();
            }

            vec_delta_ = vec_delta_ * Chev::U;

            calc_A2();

            auto eps = 0.0;
            auto nor = 0.0;
            for (auto i = 0; i < Chev::LN_2; i++) {
                eps += sqr(vec_delta_.coeff(i, i) - vec_delta_old.coeff(i, i));
                nor += sqr(vec_delta_old.coeff(i, i));
            }

            eps /= nor;

            std::cout << "ite = " << ite << ", eps = " << eps << '\n';
            if (eps <= Chev::EPSTHRESHOLD) {
                std::cout << "End " << vec_delta_.coeff(Chev::NX / 2, Chev::NY / 2) << std::endl;
                break;
            }

            vec_delta_old = vec_delta_;
        }
    }

    // #region publicメンバ関数

    // #region privateメンバ関数

    void Chev::calc_A()
    {
        A_ = Eigen::SparseMatrix<double>(Chev::LN, Chev::LN);

        for (auto ix = 0; ix < Chev::NX; ix++) {
            for (auto iy = 0; iy < Chev::NY; iy++) {
                // A_.setdiag(-mu)
                auto const ii = xy2i(ix, iy);
                auto jx = ix;
                auto jy = iy;
                auto jj = xy2i(jx, jy);
                A_.coeffRef(ii, jj) = -Chev::MYU;

                // +1 in x direction
                jx = ix + 1;

                if (jx == Chev::NX) {
                    jx = 0;
                }

                jy = iy;
                jj = xy2i(jx, jy);
                A_.coeffRef(ii, jj) = -1.0;

                // -1 in x direction
                jx = ix - 1;

                if (jx == -1) {
                    jx = Chev::NX - 1;
                }

                jy = iy;
                jj = xy2i(jx, jy);

                A_.coeffRef(ii, jj) = -1.0;

                // + 1 in y direction
                jx = ix;
                jy = iy + 1;

                if (jy == Chev::NY) {
                    jy = 0;
                }

                jj = xy2i(jx, jy);
                A_.coeffRef(ii, jj) = -1.0;

                // -1 in y direction
                jx = ix;
                jy = iy - 1;
                if (jy == -1) {
                    jy = Chev::NY - 1;
                }
                jj = xy2i(jx, jy);
                A_.coeffRef(ii, jj) = -1.0;

                for (auto i = 0; i < Chev::LN_2; i++) {
                    for (auto j = 0; j < Chev::LN_2; j++) {
                        A_.coeffRef(i + Chev::LN_2, j + Chev::LN_2) = -A_.coeff(i, j);
                        A_.coeffRef(i, j + Chev::LN_2) = vec_delta_.coeff(i, j);
                        A_.coeffRef(i + Chev::LN_2, j) = vec_delta_.coeff(j, i);
                    }
                }
            }
        }

        A_ /= Chev::AA;
    }

    void Chev::calc_A2()
    {
        A_ *= Chev::AA;

        for (auto i = 0; i < Chev::LN_2; i++) {
            for (auto j = 0; j < Chev::LN_2; j++) {
                A_.coeffRef(i, j + Chev::LN_2) = vec_delta_.coeff(i, j);
                A_.coeffRef(i + Chev::LN_2, j) = vec_delta_.coeff(j, i);
            }
        }

        A_ /= Chev::AA;
    }

    double Chev::calc_meanfield() const
    {
        auto const ba = std::acos(-Chev::BB / Chev::AA);
        auto const omeb = std::acos(-(Chev::OMEGAC + Chev::BB) / Chev::AA);

        auto density = 0.0;
        for (auto j = 0; j < Chev::NC - 1; j++) {
            auto const i = j + 1;
            density += vec_ai_[i] * (std::sin(static_cast<double>(i) * omeb) - std::sin(static_cast<double>(i) * ba)) / static_cast<double>(i);
            density += vec_ai_[0] * (omeb - ba) / 2.0;
        }

        return density * 2.0 / Chev::PI;
    }

    void Chev::calc_polynomials(std::int32_t left_i, std::int32_t right_j)
    {
        Eigen::VectorXd vec_jn(Chev::LN), vec_jnm(Chev::LN), vec_jnmm(Chev::LN);
        vec_jn.fill(0.0);
        vec_jnm.fill(0.0);
        vec_jnmm.fill(0.0);

        vec_jn.coeffRef(right_j) = 1.0;
        vec_ai_.fill(0.0);

        auto A = A_.toDense();
        for (auto n = 0; n < Chev::NC; n++) {
            switch (n) {
            case 0:
                vec_jnm.resize(1);
                vec_jnm.fill(0.0);
                vec_jnmm.resize(1);
                vec_jnmm.fill(0.0);
                vec_jn.coeffRef(right_j) = 1.0;
                break;

            case 1:
                vec_jn = A * vec_jn;
                break;

            default:
                vec_jn = 2.0 * A * vec_jnm - vec_jnmm;
                break;
            }

            vec_ai_[n] = vec_jn.coeff(left_i);
            vec_jnmm = vec_jnm;
            vec_jnm = vec_jn;
        }
    }

    void Chev::init_delta()
    {
        std::vector<Eigen::Triplet<double>> A(Chev::LN_2);
        for (auto i = 0; i < Chev::LN_2; i++) {
            A[i] = Eigen::Triplet<double>(i, i, Chev::DELTA);
        }

        vec_delta_.setFromTriplets(A.begin(), A.end());
    }

    std::int32_t Chev::xy2i(std::int32_t ix, std::int32_t iy) const
    {
        return iy * Chev::NX + ix;
    }

    // #endregion privateメンバ関数
}


// Boost.Geometry

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2014, 2016, 2017.
// Modifications copyright (c) 2014-2017 Oracle and/or its affiliates.

// 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_FORMULAS_VINCENTY_INVERSE_HPP
#define BOOST_GEOMETRY_FORMULAS_VINCENTY_INVERSE_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/formulas/differential_quantities.hpp>
#include <boost/geometry/formulas/flattening.hpp>
#include <boost/geometry/formulas/result_inverse.hpp>

#ifndef BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS
#define BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS 1000
#endif

namespace boost { namespace geometry { namespace formula
{

/*!
\brief The solution of the inverse problem of geodesics on latlong coordinates, after Vincenty, 1975
\author See
    - http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
    - http://www.icsm.gov.au/gda/gda-v_2.4.pdf
\author Adapted from various implementations to get it close to the original document
    - http://www.movable-type.co.uk/scripts/LatLongVincenty.html
    - http://exogen.case.edu/projects/geopy/source/geopy.distance.html
    - http://futureboy.homeip.net/fsp/colorize.fsp?fileName=navigation.frink

*/
template <
    typename CT,
    bool EnableDistance,
    bool EnableAzimuth,
    bool EnableReverseAzimuth = false,
    bool EnableReducedLength = false,
    bool EnableGeodesicScale = false
>
struct vincenty_inverse
{
    static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale;
    static const bool CalcAzimuths = EnableAzimuth || EnableReverseAzimuth || CalcQuantities;
    static const bool CalcFwdAzimuth = EnableAzimuth || CalcQuantities;
    static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcQuantities;

public:
    typedef result_inverse<CT> result_type;

    template <typename T1, typename T2, typename Spheroid>
    static inline result_type apply(T1 const& lon1,
                                    T1 const& lat1,
                                    T2 const& lon2,
                                    T2 const& lat2,
                                    Spheroid const& spheroid)
    {
        result_type result;

        if (math::equals(lat1, lat2) && math::equals(lon1, lon2))
        {
            return result;
        }

        CT const c1 = 1;
        CT const c2 = 2;
        CT const c3 = 3;
        CT const c4 = 4;
        CT const c16 = 16;
        CT const c_e_12 = CT(1e-12);

        CT const pi = geometry::math::pi<CT>();
        CT const two_pi = c2 * pi;

        // lambda: difference in longitude on an auxiliary sphere
        CT L = lon2 - lon1;
        CT lambda = L;

        if (L < -pi) L += two_pi;
        if (L > pi) L -= two_pi;

        CT const radius_a = CT(get_radius<0>(spheroid));
        CT const radius_b = CT(get_radius<2>(spheroid));
        CT const f = formula::flattening<CT>(spheroid);

        // U: reduced latitude, defined by tan U = (1-f) tan phi
        CT const one_min_f = c1 - f;
        CT const tan_U1 = one_min_f * tan(lat1); // above (1)
        CT const tan_U2 = one_min_f * tan(lat2); // above (1)

        // calculate sin U and cos U using trigonometric identities
        CT const temp_den_U1 = math::sqrt(c1 + math::sqr(tan_U1));
        CT const temp_den_U2 = math::sqrt(c1 + math::sqr(tan_U2));
        // cos = 1 / sqrt(1 + tan^2)
        CT const cos_U1 = c1 / temp_den_U1;
        CT const cos_U2 = c1 / temp_den_U2;
        // sin = tan / sqrt(1 + tan^2)
        // sin = tan * cos
        CT const sin_U1 = tan_U1 * cos_U1;
        CT const sin_U2 = tan_U2 * cos_U2;

        // calculate sin U and cos U directly
        //CT const U1 = atan(tan_U1);
        //CT const U2 = atan(tan_U2);
        //cos_U1 = cos(U1);
        //cos_U2 = cos(U2);
        //sin_U1 = tan_U1 * cos_U1; // sin(U1);
        //sin_U2 = tan_U2 * cos_U2; // sin(U2);

        CT previous_lambda;
        CT sin_lambda;
        CT cos_lambda;
        CT sin_sigma;
        CT sin_alpha;
        CT cos2_alpha;
        CT cos_2sigma_m;
        CT cos2_2sigma_m;
        CT sigma;

        int counter = 0; // robustness

        do
        {
            previous_lambda = lambda; // (13)
            sin_lambda = sin(lambda);
            cos_lambda = cos(lambda);
            sin_sigma = math::sqrt(math::sqr(cos_U2 * sin_lambda) + math::sqr(cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda)); // (14)
            CT cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda; // (15)
            sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma; // (17)
            cos2_alpha = c1 - math::sqr(sin_alpha);
            cos_2sigma_m = math::equals(cos2_alpha, 0) ? 0 : cos_sigma - c2 * sin_U1 * sin_U2 / cos2_alpha; // (18)
            cos2_2sigma_m = math::sqr(cos_2sigma_m);

            CT C = f/c16 * cos2_alpha * (c4 + f * (c4 - c3 * cos2_alpha)); // (10)
            sigma = atan2(sin_sigma, cos_sigma); // (16)
            lambda = L + (c1 - C) * f * sin_alpha *
                (sigma + C * sin_sigma * (cos_2sigma_m + C * cos_sigma * (-c1 + c2 * cos2_2sigma_m))); // (11)

            ++counter; // robustness

        } while ( geometry::math::abs(previous_lambda - lambda) > c_e_12
               && geometry::math::abs(lambda) < pi
               && counter < BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS ); // robustness

        if ( BOOST_GEOMETRY_CONDITION(EnableDistance) )
        {
            // Oops getting hard here
            // (again, problem is that ttmath cannot divide by doubles, which is OK)
            CT const c1 = 1;
            CT const c2 = 2;
            CT const c3 = 3;
            CT const c4 = 4;
            CT const c6 = 6;
            CT const c47 = 47;
            CT const c74 = 74;
            CT const c128 = 128;
            CT const c256 = 256;
            CT const c175 = 175;
            CT const c320 = 320;
            CT const c768 = 768;
            CT const c1024 = 1024;
            CT const c4096 = 4096;
            CT const c16384 = 16384;

            //CT sqr_u = cos2_alpha * (math::sqr(radius_a) - math::sqr(radius_b)) / math::sqr(radius_b); // above (1)
            CT sqr_u = cos2_alpha * ( math::sqr(radius_a / radius_b) - c1 ); // above (1)

            CT A = c1 + sqr_u/c16384 * (c4096 + sqr_u * (-c768 + sqr_u * (c320 - c175 * sqr_u))); // (3)
            CT B = sqr_u/c1024 * (c256 + sqr_u * ( -c128 + sqr_u * (c74 - c47 * sqr_u))); // (4)
            CT const cos_sigma = cos(sigma);
            CT const sin2_sigma = math::sqr(sin_sigma);
            CT delta_sigma = B * sin_sigma * (cos_2sigma_m + (B/c4) * (cos_sigma* (-c1 + c2 * cos2_2sigma_m)
                - (B/c6) * cos_2sigma_m * (-c3 + c4 * sin2_sigma) * (-c3 + c4 * cos2_2sigma_m))); // (6)

            result.distance = radius_b * A * (sigma - delta_sigma); // (19)
        }

        if ( BOOST_GEOMETRY_CONDITION(CalcAzimuths) )
        {
            if (BOOST_GEOMETRY_CONDITION(CalcFwdAzimuth))
            {
                result.azimuth = atan2(cos_U2 * sin_lambda, cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda); // (20)
            }

            if (BOOST_GEOMETRY_CONDITION(CalcRevAzimuth))
            {
                result.reverse_azimuth = atan2(cos_U1 * sin_lambda, -sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda); // (21)
            }
        }

        if (BOOST_GEOMETRY_CONDITION(CalcQuantities))
        {
            typedef differential_quantities<CT, EnableReducedLength, EnableGeodesicScale, 2> quantities;
            quantities::apply(lon1, lat1, lon2, lat2,
                              result.azimuth, result.reverse_azimuth,
                              radius_b, f,
                              result.reduced_length, result.geodesic_scale);
        }

        return result;
    }
};

}}} // namespace boost::geometry::formula

#endif // BOOST_GEOMETRY_FORMULAS_VINCENTY_INVERSE_HPP