/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2000 - 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, University of Heidelberg, 2000
 */

// @sect3{Include files}

// 像往常一样，前几个include文件已经知道了，所以我们将不再评论它们。

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

#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.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_handler.h>
#include <deal.II/dofs/dof_tools.h>

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

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

// 在这个例子中，我们需要矢量值的有限元。对这些的支持可以在下面的include文件中找到。

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

// 我们将用常规的Q1元素组成矢量值的有限元素，这些元素可以在这里找到，像往常一样。

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

// 这又是C++语言。

#include <fstream>
#include <iostream>

// 最后一步和以前的程序一样。特别是，就像在 step-7 中一样，我们把这个程序所特有的一切都打包到一个自己的命名空间中。

namespace Step8
{
  using namespace dealii;
// @sect3{The ElasticProblem class template}

// 主类除了名称外，与 step-6 的例子相比几乎没有变化。

// 唯一的变化是为 fe 变量使用了一个不同的类。我们现在使用的不是FE_Q这样具体的有限元类，而是一个更通用的类，FESystem。事实上，FESystem本身并不是一个真正的有限元，因为它没有实现自己的形状函数。相反，它是一个可以用来将其他几个元素堆叠在一起形成一个矢量值的有限元的类。在我们的例子中，我们将组成 FE_Q(1) 对象的矢量值元素，如下所示，在这个类的构造函数中。

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

  private:
    void setup_system();
    void assemble_system();
    void solve();
    void refine_grid();
    void output_results(const unsigned int cycle) const;

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

    FESystem<dim> fe;

    AffineConstraints<double> constraints;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

    Vector<double> solution;
    Vector<double> system_rhs;
  };
// @sect3{Right hand side values}

// 在进入主类的实现之前，我们声明并定义描述右手边的函数。这一次，右手边是向量值，解决方案也是如此，所以我们将更详细地描述为此所需的变化。

// 为了防止出现返回向量没有被设置成正确大小的情况，我们对这种情况进行了测试，否则将在函数的开始部分抛出一个异常。请注意，强制输出参数已经具有正确的大小是deal.II中的一个惯例，并且几乎在所有地方都强制执行。原因是，否则我们将不得不在函数开始时检查，并可能改变输出向量的大小。这很昂贵，而且几乎总是不必要的（对函数的第一次调用会将向量设置为正确的大小，随后的调用只需要做多余的检查）。此外，如果我们不能依赖向量已经具有正确大小的假设，那么检查和可能调整向量大小的操作是不能被删除的；这与Assert调用是一个契约，如果程序在优化模式下编译，Assert调用将被完全删除。

// 同样，如果由于某种意外，有人试图在只有一个空间维度的情况下编译和运行程序（在这种情况下，弹性方程没有什么意义，因为它们还原为普通的拉普拉斯方程），我们在第二个断言中终止程序。然而，该程序在三维空间中也能正常工作。

  template <int dim>
  void right_hand_side(const std::vector<Point<dim>> &points,
                       std::vector<Tensor<1, dim>> &  values)
  {
    Assert(values.size() == points.size(),
           ExcDimensionMismatch(values.size(), points.size()));
    Assert(dim >= 2, ExcNotImplemented());

// 该函数的其余部分实现了计算力值。我们将使用一个位于(0.5,0)和(-0.5,0)点周围的两个小圆圈（或球体，在3D中）的X方向的恒定（单位）力，以及位于原点周围的Y方向的力；在3D中，这些中心的Z分量也是零。

// 为此，让我们首先定义两个对象，表示这些区域的中心。请注意，在构建点对象时，所有的分量都被设置为零。

    Point<dim> point_1, point_2;
    point_1(0) = 0.5;
    point_2(0) = -0.5;

    for (unsigned int point_n = 0; point_n < points.size(); ++point_n)
      {

// 如果 points[point_n] 处于围绕这些点之一的半径为0.2的圆（球）中，那么将X方向的力设置为1，否则为0。

        if (((points[point_n] - point_1).norm_square() < 0.2 * 0.2) ||
            ((points[point_n] - point_2).norm_square() < 0.2 * 0.2))
          values[point_n][0] = 1.0;
        else
          values[point_n][0] = 0.0;

// 同样地，如果 points[point_n] 在原点附近，那么将y力设置为1，否则为0。

        if (points[point_n].norm_square() < 0.2 * 0.2)
          values[point_n][1] = 1.0;
        else
          values[point_n][1] = 0.0;
      }
  }

//  @sect3{The ElasticProblem class implementation}
// @sect4{ElasticProblem::ElasticProblem constructor}

// 下面是主类的构造函数。如前所述，我们想构造一个由多个标量有限元组成的矢量值有限元（即，我们想构造矢量值元素，使其每个矢量成分都由一个标量元素的形状函数组成）。当然，我们想堆叠在一起的标量有限元的数量等于解函数的分量数量，由于我们考虑每个空间方向上的位移，所以是 dim 。FESystem类可以处理这个问题：我们传递给它我们想组成系统的有限元，以及它的重复频率。

  template <int dim>
  ElasticProblem<dim>::ElasticProblem()
    : dof_handler(triangulation)
    , fe(FE_Q<dim>(1), dim)
  {}

// 事实上，FESystem类还有几个构造函数，可以进行更复杂的操作，而不仅仅是将几个相同类型的标量有限元堆叠在一起；我们将在后面的例子中了解这些可能性。

//  @sect4{ElasticProblem::setup_system}

// 设置方程组与 step-6 例子中使用的函数相同。DoFHandler类和这里使用的所有其他类都完全知道我们要使用的有限元是矢量值的，并且照顾到了有限元本身的矢量值。(事实上，它们不知道，但这不需要困扰你：因为它们只需要知道每个顶点、直线和单元有多少个自由度，它们不问它们代表什么，也就是说，考虑的有限元是矢量值的，还是例如在每个顶点上有几个自由度的标量Hermite元)。

  template <int dim>
  void ElasticProblem<dim>::setup_system()
  {
    dof_handler.distribute_dofs(fe);
    solution.reinit(dof_handler.n_dofs());
    system_rhs.reinit(dof_handler.n_dofs());

    constraints.clear();
    DoFTools::make_hanging_node_constraints(dof_handler, constraints);
    VectorTools::interpolate_boundary_values(dof_handler,
                                             0,
                                             Functions::ZeroFunction<dim>(dim),
                                             constraints);
    constraints.close();

    DynamicSparsityPattern dsp(dof_handler.n_dofs(), dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern(dof_handler,
                                    dsp,
                                    constraints,
                                    /*keep_constrained_dofs =  */ false);

    sparsity_pattern.copy_from(dsp);

    system_matrix.reinit(sparsity_pattern);
  }
// @sect4{ElasticProblem::assemble_system}

// 这个程序中最大的变化是创建矩阵和右手边，因为它们是取决于问题的。我们将一步一步地完成这个过程  step-  ，因为它比以前的例子要复杂一些。

// 然而，这个函数的前几部分和以前一样：设置一个合适的正交公式，为我们使用的（矢量值）有限元以及正交对象初始化一个FEValues对象，并声明了一些辅助数组。此外，我们还声明了永远相同的两个缩写。  n_q_points  和  dofs_per_cell  。每个单元的自由度数量，我们现在显然是从组成的有限元中询问，而不是从底层的标量Q1元中询问。在这里，它是 dim 乘以Q1元素的每个单元的自由度数，尽管这不是我们需要关心的明确知识。

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

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

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

    FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
    Vector<double>     cell_rhs(dofs_per_cell);

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

// 正如前面的例子所示，我们需要一个地方来存储单元格上所有正交点的系数值。在目前的情况下，我们有两个系数，lambda和mu。

    std::vector<double> lambda_values(n_q_points);
    std::vector<double> mu_values(n_q_points);

// 好吧，我们也可以省略上面的两个数组，因为我们将对lambda和mu使用常数系数，可以这样声明。它们都代表函数总是返回常量值1.0。尽管我们可以在矩阵的组合中省略各自的系数，但为了演示，我们在这里使用它们。

    Functions::ConstantFunction<dim> lambda(1.), mu(1.);

// 和上面的两个常量函数一样，我们将在每个单元格中只调用一次函数right_hand_side，以使事情更简单。

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

// 现在我们可以开始对所有单元格进行循环。

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

        fe_values.reinit(cell);

// 接下来我们得到正交点的系数值。同样，对于右手边也是如此。

        lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
        mu.value_list(fe_values.get_quadrature_points(), mu_values);
        right_hand_side(fe_values.get_quadrature_points(), rhs_values);

// 然后将局部刚度矩阵的条目和右手边的向量组合起来。这几乎是一对一地遵循本例介绍中描述的模式。 在位的几个评论之一是，我们可以计算数字  comp(i)  ，即使用下面的  fe.system_to_component_index(i).first  函数调用形状函数  i  的唯一非零向量成分的索引。

//（通过访问 system_to_component_index 函数返回值的 first 变量，你可能已经猜到其中还有更多的内容。事实上，该函数返回一个 std::pair@<unsigned int，无符号int @>, ，其中第一个元素是 comp(i) ，第二个元素是介绍中也指出的值 base(i) ，即这个形状函数在这个组件中所有非零的形状函数中的索引，即介绍中的字典 base(i) 。不过，这不是我们通常感兴趣的数字）。)

// 有了这些知识，我们就可以把局部矩阵的贡献集合起来。

        for (const unsigned int i : fe_values.dof_indices())
          {
            const unsigned int component_i =
              fe.system_to_component_index(i).first;

            for (const unsigned int j : fe_values.dof_indices())
              {
                const unsigned int component_j =
                  fe.system_to_component_index(j).first;

                for (const unsigned int q_point :
                     fe_values.quadrature_point_indices())
                  {
                    cell_matrix(i, j) +=

// 第一个项是  $\lambda \partial_i u_i, \partial_j v_j) + (\mu \partial_i u_j, \partial_j v_i)$  。注意， shape_grad(i,q_point) 返回正交点q_point处第i个形状函数的唯一非零分量的梯度。梯度的分量 comp(i) 是第i个形状函数的唯一非零矢量分量相对于comp(i)th坐标的导数，由附加的括号访问。

                      (                                                  //
                        (fe_values.shape_grad(i, q_point)[component_i] * //
                         fe_values.shape_grad(j, q_point)[component_j] * //
                         lambda_values[q_point])                         //
                        +                                                //
                        (fe_values.shape_grad(i, q_point)[component_j] * //
                         fe_values.shape_grad(j, q_point)[component_i] * //
                         mu_values[q_point])                             //
                        +                                                //

// 第二个项是  $(\mu \nabla u_i, \nabla v_j)$  。我们不需要访问梯度的具体分量，因为我们只需要计算两个梯度的标量乘积，这个问题由<tt>operator*</tt>的重载版本来负责，就像前面的例子一样。                            注意，通过使用<tt>?:</tt>操作符，我们只在<tt>component_i</tt>等于<tt>component_j</tt>时才这样做，否则会加上一个零（编译器会将其优化掉）。

                        ((component_i == component_j) ?        //
                           (fe_values.shape_grad(i, q_point) * //
                            fe_values.shape_grad(j, q_point) * //
                            mu_values[q_point]) :              //
                           0)                                  //
                        ) *                                    //
                      fe_values.JxW(q_point);                  //
                  }
              }
          }

// 组装右手边也和介绍中讨论的一样。

        for (const unsigned int i : fe_values.dof_indices())
          {
            const unsigned int component_i =
              fe.system_to_component_index(i).first;

            for (const unsigned int q_point :
                 fe_values.quadrature_point_indices())
              cell_rhs(i) += fe_values.shape_value(i, q_point) *
                             rhs_values[q_point][component_i] *
                             fe_values.JxW(q_point);
          }

// 从局部自由度到全局矩阵和右手向量的转移不取决于所考虑的方程，因此与之前所有的例子相同。

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

//  @sect4{ElasticProblem::solve}

// 解算器并不关心方程组的来源，只要它保持正定和对称（这是使用CG解算器的要求），而这个方程组确实是这样。因此，我们不需要改变任何东西。

  template <int dim>
  void ElasticProblem<dim>::solve()
  {
    SolverControl            solver_control(1000, 1e-12);
    SolverCG<Vector<double>> cg(solver_control);

    PreconditionSSOR<SparseMatrix<double>> preconditioner;
    preconditioner.initialize(system_matrix, 1.2);

    cg.solve(system_matrix, solution, system_rhs, preconditioner);

    constraints.distribute(solution);
  }
// @sect4{ElasticProblem::refine_grid}

// 对网格进行细化的函数与 step-6 的例子相同。正交公式再次适应了线性元素。请注意，误差估计器默认情况下是将从有限元解的所有分量中得到的估计值相加，也就是说，它使用所有方向的位移，权重相同。如果我们希望网格只适应x方向的位移，我们可以给函数传递一个额外的参数，告诉它这样做，而不考虑其他所有方向的位移作为误差指标。然而，对于目前的问题，似乎应该考虑所有的位移分量，而且权重相同。

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

    KellyErrorEstimator<dim>::estimate(dof_handler,
                                       QGauss<dim - 1>(fe.degree + 1),
                                       {},
                                       solution,
                                       estimated_error_per_cell);

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

    triangulation.execute_coarsening_and_refinement();
  }
// @sect4{ElasticProblem::output_results}

// 输出的情况与之前的例子中已经显示过的差不多了。唯一的区别是，求解函数是矢量值的。DataOut类会自动处理这个问题，但我们必须给求解向量的每个分量一个不同的名字。

// 为了做到这一点， DataOut::add_vector() 函数想要一个字符串的向量。由于分量的数量与我们工作的维数相同，我们使用下面的 switch 语句。

// 我们注意到，一些图形程序对变量名称中允许的字符有限制。因此，deal.II只支持所有程序都支持的这些字符的最小子集。基本上，这些字符是字母、数字、下划线和其他一些字符，但特别是没有空格和减号/横线。否则该库将抛出一个异常，至少在调试模式下是这样。

// 在列出了1d、2d和3d的情况后，如果我们遇到一个我们没有考虑到的情况，让程序死亡是一种很好的风格。请记住，如果第一个参数中的条件没有得到满足，Assert宏会产生一个异常。当然，条件 false 永远不可能被满足，所以只要程序运行到默认语句，就会中止。

  template <int dim>
  void ElasticProblem<dim>::output_results(const unsigned int cycle) const
  {
    DataOut<dim> data_out;
    data_out.attach_dof_handler(dof_handler);

    std::vector<std::string> solution_names;
    switch (dim)
      {
        case 1:
          solution_names.emplace_back("displacement");
          break;
        case 2:
          solution_names.emplace_back("x_displacement");
          solution_names.emplace_back("y_displacement");
          break;
        case 3:
          solution_names.emplace_back("x_displacement");
          solution_names.emplace_back("y_displacement");
          solution_names.emplace_back("z_displacement");
          break;
        default:
          Assert(false, ExcNotImplemented());
      }

// 在为解向量的不同组成部分设置了名称之后，我们可以将解向量添加到计划输出的数据向量列表中。请注意，下面的函数需要一个字符串向量作为第二个参数，而我们在以前所有例子中使用的函数在那里接受一个字符串。(事实上，我们之前使用的函数会将单个字符串转换成只有一个元素的向量，并将其转发给另一个函数)。

    data_out.add_data_vector(solution, solution_names);
    data_out.build_patches();

    std::ofstream output("solution-" + std::to_string(cycle) + ".vtk");
    data_out.write_vtk(output);
  }

//  @sect4{ElasticProblem::run}

//  run 函数所做的事情与 step-6 中的相同，比如说。这一次，我们使用平方[-1,1]^d作为域，在开始第一次迭代之前，我们在全局上对其进行了四次细化。

// 细化的原因有点意外：我们使用QGauss正交公式，在每个方向上有两个点用于整合右手边；这意味着每个单元上有四个正交点（在二维）。如果我们只对初始网格进行一次全局细化，那么在域上每个方向上就只有四个正交点。然而，右侧函数被选择为相当局部的，在这种情况下，纯属偶然，恰好所有的正交点都位于右侧函数为零的点上（用数学术语来说，正交点恰好在右侧函数的support之外的点上）。这样一来，用正交计算的右手向量将只包含零（尽管如果我们完全用积分计算右手向量的话，它当然会是非零的），方程组的解就是零向量，也就是一个处处为零的有限元函数。从某种意义上说，我们不应该对这种情况的发生感到惊讶，因为我们选择了一个完全不适合手头问题的初始网格。

// 不幸的是，如果离散解是常数，那么KellyErrorEstimator类计算的误差指标对每个单元来说也是零，对 Triangulation::refine_and_coarsen_fixed_number() 的调用将不会标记任何单元进行细化（如果每个单元的指示误差是零，为什么要这样做？因此，下一次迭代中的网格也将只由四个单元组成，同样的问题再次发生。

// 结论是：虽然我们当然不会把初始网格选择得非常适合问题的精确解决，但我们至少必须选择它，使它有机会捕捉到解决方案的重要特征。在这种情况下，它需要能够看到右手边的情况。因此，我们进行了四次全局细化。(任何更大的全局细化步骤当然也可以。)

  template <int dim>
  void ElasticProblem<dim>::run()
  {
    for (unsigned int cycle = 0; cycle < 8; ++cycle)
      {
        std::cout << "Cycle " << cycle << ':' << std::endl;

        if (cycle == 0)
          {
            GridGenerator::hyper_cube(triangulation, -1, 1);
            triangulation.refine_global(4);
          }
        else
          refine_grid();

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

        setup_system();

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

        assemble_system();
        solve();
        output_results(cycle);
      }
  }
} // namespace Step8
// @sect3{The main function}

// 在上面最后一行关闭了 Step8 命名空间后，下面是程序的主要功能，又和 step-6 中一模一样（当然，除了改变了类名）。

int main()
{
  try
    {
      Step8::ElasticProblem<2> elastic_problem_2d;
      elastic_problem_2d.run();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

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

  return 0;
}


#include <vector>

#include <NTL/ZZ.h>

#include "../src/elements/element.hpp"
#include "../src/algorithms/utils.hpp"

/* Example: using the remainder tree algorithm to search for Wilson primes.
 * This was the first application for the remainder tree algorithm and also is
 * one of the simplest. Wilson's theorem says that for all primes p, the following is true:
 *(p-1)! = -1 (mod p). A Wilson prime satisfies the same identity modulo p^2.
 *
 * Let A_0 = A_1 = 1, and A_n = n-1. Also set m_n to be n if n is prime, and 1 otherwise.
 * Then, if A_0 * A_1 * ... A_p = -1 (mod m_n), then m_n is a Wilson prime.
 */

using std::vector;

/* The first decision to make is what kind of datatype to use. In this case it is clear that we only
 * need to use integers. However, for many applications such as those in polynomial rings or if the
 * recurrence relation of the dividends is of order higher than 1, other datatypes may be desired.
 * Most of the code was written with using NTL in mind. However, we wrap every value in the type Elt.
 * Elt is a templated class object which will forward constructors, operators, etc. to its underlying type.
 * To specialize it for a new type (for example, to optimize its .mulmod() method), create a new file in
 * src/elements and then include it in elt_custom.tpp
 * I recommend specialized the methods rather than the class itself. Take a look at elt_NTL.tpp for an example.
 */
using NTL::ZZ;

//The convention here is to generate from lower bound---inclusive to upper---exclusive
vector<Elt<ZZ>> gen_n(long lower, long upper) {
    vector<Elt<ZZ>> output(upper-lower);

    for(long i = lower; i < upper; ++i) {
        if(i <= 1){
            output[i] = Elt<ZZ>(1);
        }
        else {
            output[i-lower] = Elt<ZZ> (i-1);
        }
    }
    return output;
}

vector<Elt<ZZ>> gen_second_prime_power(long lower, long upper) {
    vector<Elt<ZZ>> output(upper-lower);

    for(long i = lower; i < upper; i++){
        ZZ n(i);
        if(ProbPrime(n)) { //Technically a sieve is faster & more correct, but shouldn't make a big difference.
                            //This is just an example anyway.
            power(n, n, 2);
            output[i-lower] = Elt<ZZ>(n);
        }
        else{
            output[i-lower] = Elt<ZZ>(1);
        }
    }
    return output;
}

//TODO: explain how to modify calculate_factorial and compute V


/// @file raster_example1.cc
/// @brief raster example
/// @author Jeff Perry <jeffsp@gmail.com>
/// @version 1.0
/// @date 2013-01-14

#include "jack_rabbit/jack_rabbit.h"
#include <algorithm>
#include <boost/lambda/lambda.hpp>
#include <cmath>
#include <complex>
#include <fstream>
#include <iostream>

using namespace boost::lambda;
using namespace jack_rabbit;
using namespace std;

// Affine transformation helper
class Affine
{
    public:
    Affine ()
        : t_ (0), s_ (1)
    { }
    // Translate by 't' and scale by 's'
    Affine (float t, float s)
        : t_ (t), s_ (s)
    { }
    float To (size_t x) const
    { return t_ + x * s_; }
    size_t From (float x) const
    { return static_cast<int> ((x - t_) / s_); }
    private:
    float t_, s_;
};

// Mandelbrot function object:
// Map the Mandelbrot set onto a raster
template<typename T>
class Mandelbrot
{
    public:
    Mandelbrot (size_t /*r*/, size_t /*c*/) { }
    // Define coordinate mapping
    void CoordMap (const Affine &ax, const Affine &ay)
    { ax_ = ax; ay_ = ay; }
    // Remap coordinate (r, c), and then determine if it's
    // in the set or not.
    T operator() (size_t r, size_t c) const
    {
        // Remap x and y
        complex<float> z (ax_.To (c), ay_.To (r));
        complex<float> k (z);
        size_t count = 0;
        // Iterate until we are kindof sure that the point
        // is in the set, or until the point is sure to
        // shoot off to infinity...
        while (++count != ITER)
        {
            z = z * z + k;
            if (norm (z) > 4.0)
                // ... definitely not in the set
                return OUT;
        }
        // ... probably in the set.
        return IN;
    }
    private:
    static const size_t ITER = 500;
    static const T IN = 0;
    static const T OUT = 255;
    Affine ax_, ay_;
};

// PGM file writer helper
template<class T>
void WritePGM (const T &m, ofstream &ofs)
{
    // Write a pgm header
    ofs << "P5\n"
        << "# Raster Example\n"
        << m.cols () << ' ' << m.rows () << '\n'
        << "255\n";

    // Transform to a vector of chars
    vector<char> mm (m.begin (), m.end ());

    // Write the pgm pixels
    const std::streamsize sz =
        static_cast<std::streamsize> (mm.size ());
    ofs.write (&mm[0], sz);
}

int main ()
{
    try
    {
        const size_t M = 256;
        const size_t N = 256;

        clog << "Generating a " << M << "X" << N
            << " image..." << endl;

        typedef raster<int> Image;

        // Some matrices that will contain our images
        Image m0 (M, N);
        Image m1 (M / 3, N / 3);

        // Our Mandelbrot functions
        subscript_generator<Image::value_type,Mandelbrot>
        mandel0 (m0.rows (), m0.cols ());
        Affine ax0 (-2.2f, 3.0f / m0.rows ());
        Affine ay0 (-1.7f, 3.0f / m0.rows ());
        mandel0.CoordMap (ax0, ay0);

        subscript_generator<Image::value_type,Mandelbrot>
        mandel1 (m1.rows (), m1.cols ());
        Affine ax1 (-0.28f, 0.30f / m1.rows ());
        Affine ay1 (-0.92f, 0.30f / m1.rows ());
        mandel1.CoordMap (ax1, ay1);

        // Create the main image
        generate (m0.begin (), m0.end (), mandel0);

        // Lower contrast of the region to zoom
        size_t r1 = ay0.From (ay1.To (0));
        size_t c1 = ax0.From (ax1.To (0));
        size_t r2 = ay0.From (ay1.To (m1.rows ()));
        size_t c2 = ax0.From (ax1.To (m1.cols ()));
        subregion s0 = m0.sub (
            static_cast<int> (r1), static_cast<int> (c1),
            r2 - r1, c2 - c1);
        for_each (m0.begin (s0), m0.end (s0),
            _1 = (_1 - 127) / 2 + 127);

        // Create the zoomed image
        generate (m1.begin (), m1.end (), mandel1);
        // Copy the zoomed image to the main image...
        subregion s1 = { m0.rows () / 9, m0.rows () / 9,
            m1.rows (), m1.cols () };
        copy (m1.begin (), m1.end (), m0.begin (s1));

        // Lower contrast of the zoomed image
        transform (m0.begin (s1), m0.end (s1), m0.begin (s1),
            (_1 - 127) / 2 + 127);

        // Save the image to a file
        const string fn ("raster_example1.pgm");
        clog << "Writing image to " << fn << "..." << endl;

        ofstream ofs (fn.c_str ());
        if (!ofs)
            throw std::runtime_error ("Could not open file for writing");

        WritePGM (m0, ofs);

        return 0;
    }
    catch (const exception &e)
    {
        cerr << e.what () << endl;
        return -1;
    }
}


/*
 * Tools.cpp
 *
 *  Created on: Apr 22, 2014
 *      Author: wilfeli
 */

#include <vector>
#include <math.h>
#include <algorithm>
#include <iterator>
#include <Eigen/Dense>
#include <iostream>
#include "Tools.h"

using Eigen::MatrixXd;
using std::vector;

namespace Tools{

void
mgrid(MatrixXd grid, MatrixXd* meshgrid){
	//creates meshgrid from vector of points
	//define number of steps for each input
	long N_rows = grid.rows();
	int n_steps_i;
	double n_steps_i_int;
	int j_val;

	double val;
	vector<int> n_steps;
	vector<int> ac_n_steps;
	vector<vector<double>> values;
	vector<double> values_temp;
	ac_n_steps.push_back(1);

    //create list of values
	for (int i=0; i < N_rows; i++){
		//n_steps
		n_steps_i_int = (grid(i,1) - grid(i,0))/grid(i,2);

		n_steps_i = static_cast<int>(floor(n_steps_i_int)) + 1;
		n_steps.push_back(n_steps_i);
		ac_n_steps.push_back(ac_n_steps.back() * n_steps_i);

		for (int j=0; j<n_steps_i; j++){
			val = grid(i,0) + grid(i,2)*j;
			values_temp.push_back(val);
		};

		values.push_back(values_temp);
		values_temp.clear();
	};

	//create matrix and fill it with values
	(*meshgrid) = MatrixXd::Zero(N_rows, ac_n_steps.back());

	int step_length = ac_n_steps.back();
	for (int i = 0; i < N_rows; i++){
		step_length = step_length / n_steps[i];
		for (int j = 0; j < ac_n_steps.back(); j++){
			j_val = ((int)(j/step_length))%n_steps[i];
			(*meshgrid)(i,j) = values[i][j_val];

		};
	};

//	std::cout << (*meshgrid).transpose() << "\n";

};

void
mgrid_test(MatrixXd grid, MatrixXd* meshgrid){
    //creates meshgrid from vector of points
    //reverse to correspond to python
    long N_rows = grid.rows();
    int n_steps_i;
    double n_steps_i_int;
    int j_val;

    double val;
    vector<int> n_steps;
    vector<int> ac_n_steps;
    vector<vector<double>> values;
    vector<double> values_temp;
    ac_n_steps.push_back(1);

    for (int i=0; i < N_rows; i++){
        //n_steps
        n_steps_i_int = (grid(i,1) - grid(i,0))/grid(i,2);

        n_steps_i = static_cast<int>(floor(n_steps_i_int)) + 1;
        n_steps.push_back(n_steps_i);
        ac_n_steps.push_back(ac_n_steps.back() * n_steps_i);

        for (int j=0; j<n_steps_i; j++){
            val = grid(i,0) + grid(i,2)*j;
            values_temp.push_back(val);
        };

        values.push_back(values_temp);
        values_temp.clear();
    };

    //create matrix and fill it with values
    (*meshgrid) = MatrixXd::Zero(N_rows, ac_n_steps.back());

    int step_length = 1;
    for (int i = 0; i < N_rows; i++){

        for (int j = 0; j < ac_n_steps.back(); j++){
            j_val = ((int)(j/step_length))%n_steps[i];
            (*meshgrid)(i,j) = values[i][j_val];

        };
        step_length = step_length * n_steps[i];
    };

//    std::cout << (*meshgrid).transpose() << "\n";

};

void
print_vector(std::vector<double> vec){
	//print all q
	for (auto iter:vec){
		std::cout << iter << " ";
	};

    std::cout << std::endl;
};

void
print_vector(std::vector<std::vector<double>> vec){
    //print all q
    for (auto iter:vec){
        print_vector(iter);
    };

};

MyRNG::MyRNG(double seed_):state(seed_){
    m_w = seed_;
};

// Produce a uniform random sample from the open interval (0, 1).
// The method will not return either end point.
double
MyRNG::GetUniform(){
    // 0 <= u < 2^32
	uint64_t u = GetUint();
    // The magic number below is 1/(2^32 + 2).
    // The result is strictly between 0 and 1.
    return (u + 1.0) * 2.328306435454494e-10;
};

// This is the heart of the generator.
// It uses George Marsaglia's MWC algorithm to produce an unsigned integer.
// See http://www.bobwheeler.com/statistics/Password/MarsagliaPost.txt

uint64_t
MyRNG::GetUint(){
    m_z = 36969 * (m_z & 65535) + (m_z >> 16);
    m_w = 18000 * (m_w & 65535) + (m_w >> 16);
    return (m_z << 16) + m_w;
};

double
get_normal(double mean, double sigma, MyRNG& rng){
    if(rng.hasSpare)
	{
		rng.hasSpare = false;
		return sigma * sqrt(rng.rn1) * sin(rng.rn2) + mean;
	}

	rng.hasSpare = true;

	rng.rn1 = rng.GetUniform();
	if(rng.rn1 < 1e-100) {rng.rn1 = 1e-100;};
	rng.rn1 = -2 * log(rng.rn1);
	rng.rn2 = rng.GetUniform() * M_PI * 2;

	return sigma*sqrt(rng.rn1) * cos(rng.rn2) + mean;

};

int
get_int(int min, int max, MyRNG& rng){
	uint64_t x_raw = rng.GetUint();
    //clean state of rng to simplify code
    rng.hasSpare = false;

    int x = min + (x_raw % (int)(max - min + 1));

    return x;

};

};


/******************************************************************************

  This source file is part of the Avogadro project.

  Copyright (C) 2010 Eric C. Brown

  This source code is released under the New BSD License, (the "License").

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.

******************************************************************************/

#include "qtaimmathutilities.h"

#include <cmath>
#include <Eigen/Eigenvalues>
#include <Eigen/QR>

namespace Avogadro {
namespace QtPlugins {
namespace QTAIMMathUtilities {

  Matrix<qreal,3,1> eigenvaluesOfASymmetricThreeByThreeMatrix(const Matrix<qreal,3,3> &A)
  {
    SelfAdjointEigenSolver<Matrix<qreal, 3, 3> > eigensolver(A);
    return eigensolver.eigenvalues();
  }

  Matrix<qreal,3,3> eigenvectorsOfASymmetricThreeByThreeMatrix(const Matrix<qreal,3,3> &A)
  {
    SelfAdjointEigenSolver<Matrix<qreal, 3, 3> > eigensolver(A);
    return eigensolver.eigenvectors();
  }

  Matrix<qreal,4,1> eigenvaluesOfASymmetricFourByFourMatrix(const Matrix<qreal,4,4> &A)
  {
    SelfAdjointEigenSolver<Matrix<qreal, 4, 4> > eigensolver(A);
    return eigensolver.eigenvalues();
  }

  Matrix<qreal,4,4> eigenvectorsOfASymmetricFourByFourMatrix(const Matrix<qreal,4,4> &A)
  {
    SelfAdjointEigenSolver<Matrix<qreal, 4, 4> > eigensolver(A);
    return eigensolver.eigenvectors();
  }

  qint64 signOfARealNumber(qreal x)
  {
    if (x > 0.)
      return  1;
    else if (x == 0.)
      return  0;
    else
      return -1;
  }

  qint64 signatureOfASymmetricThreeByThreeMatrix(const Matrix<qreal,3,3> &A)
  {
    SelfAdjointEigenSolver<Matrix<qreal, 3, 3> > eigensolver(A);
    Matrix<qreal,3,1> eigenvalues=eigensolver.eigenvalues();

    return signOfARealNumber(eigenvalues(0)) +
        signOfARealNumber(eigenvalues(1)) +
        signOfARealNumber(eigenvalues(2));
  }

  qreal ellipticityOfASymmetricThreeByThreeMatrix(const Matrix<qreal,3,3> &A)
  {
    SelfAdjointEigenSolver<Matrix<qreal, 3, 3> > eigensolver(A);
    Matrix<qreal, 3, 1> eigenvalues=eigensolver.eigenvalues();

    return (eigenvalues(0) / eigenvalues(1)) - 1.0 ;
  }

  qreal distance(const Matrix<qreal, 3, 1> &a, const Matrix<qreal, 3, 1> &b)
  {
    return sqrt(pow(a(0) - b(0), 2) +
                pow(a(1) - b(1), 2) +
                pow(a(2) - b(2), 2));
  }

  Matrix<qreal,3,1> sphericalToCartesian(const Matrix<qreal,3,1> &rtp,
                                         const Matrix<qreal,3,1> &x0y0z0)
  {
    qreal r=rtp(0);
    qreal theta=rtp(1);
    qreal phi=rtp(2);

    qreal x0=x0y0z0(0);
    qreal y0=x0y0z0(1);
    qreal z0=x0y0z0(2);

    qreal costheta = cos(theta);
    qreal cosphi   = cos(phi);
    qreal sintheta = sin(theta);
    qreal sinphi   = sin(phi);

    Matrix<qreal, 3, 1> xyz(r * cosphi * sintheta + x0,
                            r * sintheta * sinphi + y0,
                            r * costheta          + z0);

    return xyz;
  }

  Matrix<qreal, 3, 1> sphericalToCartesian(const Matrix<qreal, 3, 1> &rtp)
  {
    Matrix<qreal, 3, 1> x0y0z0(0., 0., 0.);

    return  sphericalToCartesian(rtp, x0y0z0);
  }

  Matrix<qreal,3,1> cartesianToSpherical(const Matrix<qreal, 3, 1> &xyz,
                                         const Matrix<qreal, 3, 1> &x0y0z0 )
  {
    qreal x=xyz(0);
    qreal y=xyz(1);
    qreal z=xyz(2);

    qreal x0=x0y0z0(0);
    qreal y0=x0y0z0(1);
    qreal z0=x0y0z0(2);

    qreal xshift = x - x0;
    qreal yshift = y - y0;
    qreal zshift = z - z0;

    qreal length = sqrt(pow(xshift, 2) + pow(yshift, 2) + pow(zshift, 2));

    Matrix<qreal, 3, 1> rtp;

    if (length == 0.)
      rtp << x0, y0, z0 ;
    else if (xshift == 0. && yshift == 0.)
      rtp << length, acos(zshift / length), 0.;
    else
      rtp << length, acos(zshift / length), atan2(xshift, yshift);

    return rtp;
  }

  Matrix<qreal, 3, 1> cartesianToSpherical(const Matrix<qreal, 3, 1> &xyz)
  {
    Matrix<qreal, 3, 1> x0y0z0(0., 0., 0.);

    return  cartesianToSpherical(xyz, x0y0z0);
  }

  // Cerjan-Miller-Baker-Popelier Methods
  //
  // Based on:
  // Popelier, P.L.A. Comput. Phys. Comm. 1996, 93, 212.

  Matrix<qreal, 3, 1> minusThreeSignatureLocatorGradient(const Matrix<qreal, 3, 1> &g,
                                                         const Matrix<qreal, 3, 3> &H)
  {
    Matrix<qreal, 3, 1> value;

    Matrix<qreal, 3, 1> b = eigenvaluesOfASymmetricThreeByThreeMatrix(H);
    Matrix<qreal, 3, 3> U = eigenvectorsOfASymmetricThreeByThreeMatrix(H);

    Matrix<qreal, 3, 1> F = U.transpose() * g;

    Matrix<qreal, 4, 4> A;
    A <<  b(0), 0.  , 0.  , F(0),
    0.  , b(1), 0.  , F(1),
    0.  , 0.  , b(2), F(2),
    F(0), F(1), F(2), 0.   ;

    Matrix<qreal, 4, 1> eval = eigenvaluesOfASymmetricFourByFourMatrix(A);

    Matrix<qreal, 3, 1> lambda;
    lambda << eval(3), eval(3), eval(3);

    Matrix<qreal, 3, 1> denom;
    denom = b - lambda;

    for (qint64 i=0; i < 3; ++i)
      if( denom(i) < SMALL )
        denom(i)=denom(i)+SMALL;

    Matrix<qreal, 3, 1> h;
    h << 0., 0., 0.;

    for (qint64 j = 0; j < 3; ++j)
      for (qint64 i = 0; i < 3; ++i)
        h(j) = h(j) + ( -F(i) * U(j, i) ) / denom(i);

    value = h;

    return value;
  }

  Matrix<qreal, 3, 1> minusOneSignatureLocatorGradient(const Matrix<qreal, 3, 1> &g,
                                                       const Matrix<qreal,3,3> &H)
  {
    Matrix<qreal, 3, 1> value;

    Matrix<qreal, 3, 1> b = eigenvaluesOfASymmetricThreeByThreeMatrix(H);
    Matrix<qreal, 3, 3> U = eigenvectorsOfASymmetricThreeByThreeMatrix(H);

    Matrix<qreal, 3, 1> F = U.transpose() * g;

    Matrix<qreal, 3, 3> A;
    A <<  b(0), 0.  ,  F(0),
    0.  , b(1),  F(1),
    F(0), F(1),  0.   ;

    Matrix<qreal, 3, 1> eval = eigenvaluesOfASymmetricThreeByThreeMatrix(A);

    Matrix<qreal, 3, 1> lambda;
    lambda << eval(2), eval(2), (0.5) * (b(2) - sqrt(pow(b(2), 2) + 4.0 * pow(F(2), 2)));

    Matrix<qreal, 3, 1> denom;
    denom = b - lambda;

    for (qint64 i = 0; i < 3; ++i)
      if (denom(i) < SMALL)
        denom(i) = denom(i) + SMALL;

    Matrix<qreal, 3, 1> h;
    h << 0., 0., 0.;

    for (qint64 j = 0; j < 3; ++j)
      for (qint64 i = 0; i < 3; ++i)
        h(j) = h(j) + (-F(i) * U(j,i)) / denom(i);

    value = h;

    return value;
  }

  Matrix<qreal, 3, 1> plusOneSignatureLocatorGradient(const Matrix<qreal, 3, 1> &g,
                                                      const Matrix<qreal,3,3> &H)
  {
    Matrix<qreal, 3, 1> value;

    Matrix<qreal, 3, 1> b = eigenvaluesOfASymmetricThreeByThreeMatrix(H);
    Matrix<qreal, 3, 3> U = eigenvectorsOfASymmetricThreeByThreeMatrix(H);

    Matrix<qreal, 3, 1> F = U * g;

    Matrix<qreal, 3, 3> A;
    A <<  b(1), 0.  ,  F(1),
    0.  , b(2),  F(2),
    F(1), F(2),  0.;

    Matrix<qreal, 3, 1> eval = eigenvaluesOfASymmetricThreeByThreeMatrix(A);

    Matrix<qreal, 3, 1> lambda;
    lambda << eval(2), eval(2), (0.5) * (b(0) + sqrt(pow(b(0), 2) + 4.0 * pow(F(0), 2)));

    Matrix<qreal, 3, 1> denom;
    denom = b - lambda;

    for (qint64 i = 0; i < 3; ++i)
      if (denom(i) < SMALL)
        denom(i) = denom(i) + SMALL;

    Matrix<qreal, 3, 1> h;
    h << 0., 0., 0.;

    for (qint64 j = 0; j < 3; ++j)
      for (qint64 i = 0; i < 3; ++i)
        h(j) = h(j) + (-F(i) * U(i, j)) / denom(i);

    value = h;

    return value;
  }

  Matrix<qreal,3,1> plusThreeSignatureLocatorGradient(const Matrix<qreal, 3, 1> &g,
                                                      const Matrix<qreal, 3, 3> &H)
  {
    Matrix<qreal, 3, 1> value;

    Matrix<qreal, 3, 1> b = eigenvaluesOfASymmetricThreeByThreeMatrix(H);
    Matrix<qreal, 3, 3> U = eigenvectorsOfASymmetricThreeByThreeMatrix(H);

    Matrix<qreal, 3, 1> F = U * g;

    Matrix<qreal, 4, 4> A;
    A <<  b(0), 0.  , 0.  , F(0),
    0.  , b(1), 0.  , F(1),
    0.  , 0.  , b(2), F(2),
    F(0), F(1), F(2), 0.;

    Matrix<qreal, 4, 1> eval = eigenvaluesOfASymmetricFourByFourMatrix(A);

    Matrix<qreal, 3, 1> lambda;
    lambda << eval(0), eval(0), eval(0);

    Matrix<qreal, 3, 1> denom;
    denom = b - lambda;

    for (qint64 i = 0; i < 3; ++i)
      if (denom(i) < SMALL)
        denom(i) = denom(i) + SMALL;

    Matrix<qreal, 3, 1> h;
    h << 0., 0., 0.;

    for (qint64 j = 0; j < 3; ++j)
      for (qint64 i = 0; i < 3; ++i)
        h(j) = h(j) + (-F(i) * U(i, j) ) / denom(i);

    value = h;

    return value;
  }

} // namespace QTAIMMathUtilities
} // namespace QtPlugins
} // namespace Avogadro


#include <cassert>
#include <cmath>
#include <complex>
#include <vector>
#include <Eigen/Core>

#include "Const.h"
#include "Tract.h"

using namespace std;
using namespace Eigen;

Tract::Tract(double c1, double dl, const double S[], int length) :
	a	(130 * Const::pi),
	b	(std::pow(30 * Const::pi, 2)),
	om0	(pow(406 * Const::pi, 2)),
	c1	(c1),
	dl	(dl),
	S	(length)
{
	for (int i = 0; i < length; i++)
		this->S[i] = S[i];
}
Tract::Tract(double c1, double dl, vector<double> &S) :
	a	(130 * Const::pi),
	b	(std::pow(30 * Const::pi, 2)),
	om0	(pow(406 * Const::pi, 2)),
	c1	(c1),
	dl	(dl),
	S	(S)
{
}

Tract::~Tract(void)
{
}

complex<double> Tract::GetBeta(double f)
{
	double omg = 2.0 * Const::pi * f;
	complex<double> s(0, omg);
	return s * om0 / ((s + a) * s + b) + sqrt(s * c1);
}

vector<double> Tract::GetArea(void)
{
	return S;
}

double Tract::GetStartArea(void)
{
	return S.front();
}

double Tract::GetEndArea(void)
{
	return S.back();
}

double Tract::GetElemLength(void)
{
	return dl;
}

Matrix2cd Tract::ChainMatrix(double freq, int from)
{
	return ChainMatrix(freq, from, S.size() - 1);
}
Matrix2cd Tract::ChainMatrix(double freq, int from, int to)
{
	double n = to - from + 1;

	double omg = 2.0 * Const::pi * freq;
	complex<double> s(0, omg);

	complex<double> loss = s * om0 / ((s + a) * s + b) + sqrt(s * c1);
	complex<double> gamma = sqrt((a + s) / (loss + s));
	complex<double> sigma = gamma * (loss + s) / Const::c;

	Matrix2cd r = Matrix2cd::Identity();
	complex<double> sh = sinh(sigma * dl);
	Matrix2cd K;
    K(0, 0) = K(1, 1) = cosh(sigma * dl);

    for (int i = 0; i < n; i++)
    {
        complex<double> coeff = -Const::rho * Const::c / S[from + i] * gamma;

		K(0, 1) = coeff * sh;
        K(1, 0) = 1.0 / coeff * sh;

        r = K * r;
    }

    return r;
}


// solving A * X = B
// using driver function gesv()
// with c_vector<> & c_matrix<>

#include <cstddef>
#include <iostream>
#include <boost/numeric/bindings/lapack/driver/gesv.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/std/vector.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/ublas/io.hpp>

namespace ublas = boost::numeric::ublas;
namespace bindings = boost::numeric::bindings;
namespace lapack = boost::numeric::bindings::lapack;

using std::size_t;
using std::cout;
using std::endl;

typedef ublas::c_matrix<double, 4, 4> m4x4_t;
typedef ublas::c_matrix<double, 3, 3> m3x3_t;
typedef ublas::c_matrix<double, 2, 3> mrhs_t;
typedef ublas::c_vector<double, 5> v5_t;

int main() {

  cout << endl;
  size_t n = 3;

  m4x4_t a (n, n);   // system matrix
  m4x4_t a_copy (n, n);   // system matrix

//       [,1] [,2] [,3]
//  [1,]    1    1    1
//  [2,]    2    3    1
//  [3,]    1   -1   -1

  a(0,0) = 1.; a(0,1) = 1.; a(0,2) = 1.;
  a(1,0) = 2.; a(1,1) = 3.; a(1,2) = 1.;
  a(2,0) = 1.; a(2,1) = -1.; a(2,2) = -1.;
  a_copy = a;

  mrhs_t b (2, n);  // right-hand side matrix

//          [,1] [,2]
//     [1,]    4   10
//     [2,]    9   11
//     [3,]    2   12

  b(0,0) =  4.;  b(1,0) = 10.;
  b(0,1) =  9.;  b(1,1) = 11.;
  b(0,2) = -2.;  b(1,2) = 12.;

  m3x3_t a2; // for part 2
  a2 = project (a, ublas::range (0,3), ublas::range (0,3));
  v5_t b2 (n);
  b2 = row (b, 0);

  // part 1:
  cout << "A: " << a << endl;
  cout << "B: " << ublas::trans(b) << endl;

  std::vector< int > pivota( 100 );
  lapack::gesv (a, pivota, bindings::trans(b));

  cout << "X: " << ublas::trans(b) << endl;
  std::cout << "---" << std::endl;
  cout << "A: " << a << endl;

  cout << "AX/B: " << ublas::prod( a_copy, ublas::trans(b) ) << std::endl;

  cout << endl;

  // part 2:
  cout << "A: " << a2 << endl;
  cout << "B: " << b2 << endl;

  std::vector< int > pivota2( 100 );
 lapack::gesv (a2, pivota2, b2);
  cout << "X: " << b2 << endl;

  cout << endl;
}


#pragma once

// Code adapted from https://github.com/propanoid/DBSCAN

#include <vector>
#include <algorithm>
//#include <omp.h>

// Any basic vector/matrix library should also work
#include <Eigen/Core>

namespace clustering
{
	template<typename Vector, typename Matrix>
	class DBSCAN
	{
	public:
		typedef Vector FeaturesWeights;
		typedef Matrix ClusterData;
		typedef Matrix DistanceMatrix;
		typedef std::vector<unsigned int> Neighbors;
		typedef std::vector<int> Labels;

	private:
		double m_eps;
		size_t m_min_elems;
		double m_dmin;
		double m_dmax;

		Labels m_labels;

	public:

		// 'eps' is the search space for neighbors in the range [0,1], where 0.0 is exactly self and 1.0 is entire dataset
		DBSCAN(double eps, size_t min_elems)
			: m_eps( eps )
			, m_min_elems( min_elems )
			, m_dmin(0.0)
			, m_dmax(0.0)
		{
			reset();
		}

		// Call this to perform clustering, get results by calling 'get_labels()'
		void fit( const ClusterData & C )
		{
			const FeaturesWeights W = std_weights( C.cols() );
			wfit( C, W );
		}

		const Labels & get_labels() const
		{
			return m_labels;
		}

		void reset()
		{
			m_labels.clear();
		}

		void init(double eps, size_t min_elems)
		{
			m_eps = eps;
			m_min_elems = min_elems;
		}

		// Useful for testing
		static ClusterData gen_cluster_data( size_t features_num, size_t elements_num )
		{
			ClusterData cl_d( elements_num, features_num );
			for (size_t i = 0; i < elements_num; ++i)
				for (size_t j = 0; j < features_num; ++j)
					cl_d(i, j) = (-1.0 + rand() * (2.0) / RAND_MAX);
			return cl_d;
		}

		FeaturesWeights std_weights( size_t s )
		{
			// num cols
			FeaturesWeights ws( s );

			for (size_t i = 0; i < s; ++i)
				ws(i) = 1.0;

			return ws;
		}

		void fit_precomputed( const DistanceMatrix & D )
		{
			prepare_labels( D.rows() );
			dbscan( D );
		}

		void wfit( const ClusterData & C, const FeaturesWeights & W )
		{
			prepare_labels( C.rows() );
			const DistanceMatrix D = calc_dist_matrix( C, W );
			dbscan( D );
		}

	private:
		void prepare_labels( size_t s )
		{
			m_labels.resize(s, -1);
		}

		Neighbors find_neighbors(const DistanceMatrix & D, unsigned int pid)
		{
			Neighbors ne;

			for (unsigned int j = 0; j < D.rows(); ++j)
			{
				if 	( D(pid, j) <= m_eps )
				{
					ne.push_back(j);
				}
			}
			return ne;
		}

		const DistanceMatrix calc_dist_matrix( const ClusterData & C, const FeaturesWeights & W )
		{
			ClusterData cl_d = C;

//#pragma omp parallel for
			for (int i = 0; i < (int)cl_d.cols(); ++i)
			{
				auto col = cl_d.col(i);

				const auto r = std::minmax_element( col.data(), col.data() + col.size() );

				double data_min = *r.first;
				double data_range = *r.second - *r.first;

				if (data_range == 0.0) { data_range = 1.0; }

				const double scale = 1/data_range;
				const double min = -1.0*data_min*scale;

				col *= scale;
				col += Vector::Constant(col.size(), min);

				cl_d.col(i) = col;
			}

			// rows x rows
			DistanceMatrix d_m( cl_d.rows(), cl_d.rows() );
			Vector d_max( cl_d.rows() );
			Vector d_min( cl_d.rows() );

			for (int i = 0; i < (int)cl_d.rows(); ++i)
			{
//#pragma omp parallel for
				for (int j = i; j < (int)cl_d.rows(); ++j)
				{
					d_m(i, j) = 0.0;

					if (i != j)
					{
						Vector U = cl_d.row(i);
						Vector V = cl_d.row(j);

						Vector diff = ( U-V );

						for(int k = 0; k < (int)diff.size(); k++)
						{
							auto e = diff[k];
							d_m(i, j) += fabs(e)*W[k];
						}

						d_m(j, i) = d_m(i, j);
					}
				}

				const auto cur_row = d_m.row(i);
				const auto mm = std::minmax_element( cur_row.data(), cur_row.data() + cur_row.size() );

				d_max(i) = *mm.second;
				d_min(i) = *mm.first;
			}

			m_dmin = *(std::min_element( d_min.data(), d_min.data() + d_min.size() ));
			m_dmax = *(std::max_element( d_max.data(), d_max.data() + d_max.size() ));

			m_eps = (m_dmax - m_dmin) * m_eps + m_dmin;

			return d_m;
		}

		void dbscan( const DistanceMatrix & dm )
		{
			std::vector<unsigned int> visited( dm.rows() );

			unsigned int cluster_id = 0;

			for (unsigned int pid = 0; pid < dm.rows(); ++pid)
			{
				if ( !visited[pid] )
				{
					visited[pid] = 1;

					Neighbors ne = find_neighbors(dm, pid );

					if (ne.size() >= m_min_elems)
					{
						m_labels[pid] = cluster_id;

						for (unsigned int i = 0; i < ne.size(); ++i)
						{
							unsigned int nPid = ne[i];

							if ( !visited[nPid] )
							{
								visited[nPid] = 1;

								Neighbors ne1 = find_neighbors(dm, nPid);

								if ( ne1.size() >= m_min_elems )
								{
									for (const auto & n1 : ne1)
									{
										ne.push_back(n1);
									}
								}
							}

							if ( m_labels[nPid] == -1 )
							{
								m_labels[nPid] = cluster_id;
							}
						}

						++cluster_id;
					}
				}
			}
		}
	};
}


// https://www.codechef.com/problems/FCTRL2

#include <bits/stdc++.h>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;

int main(){
	int t;
	cin>>t;
	while(t--){
		int n;
		cin>>n;
		cpp_int fact = 1;
		for(int i=1; i<=n; i++){
			fact *= i;
		}
		cout<<fact<<endl;
	}
}


#include <iostream>
#include <vector>
#include <fstream>

using namespace std;

#include <boost/timer.hpp>

// for sophus
#include <sophus/se3.hpp>

using Sophus::SE3d;

// for eigen
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace Eigen;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "plot.h"

using namespace cv;

/**********************************************
* 本程序演示了单目相机在已知轨迹下的稠密深度估计
* 使用极线搜索 + NCC 匹配的方式，与书本的 12.2 节对应
* 请注意本程序并不完美，你完全可以改进它——我其实在故意暴露一些问题(这是借口)。
***********************************************/
std::ofstream debug("debug.txt");
// ------------------------------------------------------------------
// parameters
const int boarder = 20;         // 边缘宽度
const int width = 640;          // 图像宽度
const int height = 480;         // 图像高度
const double fx = 481.2f;       // 相机内参
const double fy = -480.0f;
const double cx = 319.5f;
const double cy = 239.5f;
const int ncc_window_size = 3;    // NCC 取的窗口半宽度
const int ncc_area = (2 * ncc_window_size + 1) * (2 * ncc_window_size + 1); // NCC窗口面积
const double min_cov = 0.1;     // 收敛判定：最小方差
const double max_cov = 10;      // 发散判定：最大方差

// ------------------------------------------------------------------
// 重要的函数
/// 从 REMODE 数据集读取数据
bool readDatasetFiles(
    const string &path,
    vector<string> &color_image_files,
    vector<SE3d> &poses,
    cv::Mat &ref_depth
);

/**
 * 根据新的图像更新深度估计
 * @param ref           参考图像
 * @param curr          当前图像
 * @param T_C_R         参考图像到当前图像的位姿
 * @param depth         深度
 * @param depth_cov     深度方差
 * @return              是否成功
 */
void update(
    const Mat &ref,
    const Mat &curr,
    const SE3d &T_C_R,
    Mat &depth,
    Mat &depth_cov2
);

/**
 * 极线搜索
 * @param ref           参考图像
 * @param curr          当前图像
 * @param T_C_R         位姿
 * @param pt_ref        参考图像中点的位置
 * @param depth_mu      深度均值
 * @param depth_cov     深度方差
 * @param pt_curr       当前点
 * @param epipolar_direction  极线方向
 * @return              是否成功
 */
bool epipolarSearch(
    const Mat &ref,
    const Mat &curr,
    const SE3d &T_C_R,
    const Vector2d &pt_ref,
    const double &depth_mu,
    const double &depth_cov,
    Vector2d &pt_curr,
    Vector2d &epipolar_direction
);

/**
 * 更新深度滤波器
 * @param pt_ref    参考图像点
 * @param pt_curr   当前图像点
 * @param T_C_R     位姿
 * @param epipolar_direction 极线方向
 * @param depth     深度均值
 * @param depth_cov2    深度方向
 * @return          是否成功
 */
bool updateDepthFilter(
    const Vector2d &pt_ref,
    const Vector2d &pt_curr,
    const SE3d &T_C_R,
    const Vector2d &epipolar_direction,
    Mat &depth,
    Mat &depth_cov2
);

/**
 * 计算 NCC 评分
 * @param ref       参考图像
 * @param curr      当前图像
 * @param pt_ref    参考点
 * @param pt_curr   当前点
 * @return          NCC评分
 */
double NCC(const Mat &ref, const Mat &curr, const Vector2d &pt_ref, const Vector2d &pt_curr);

// 双线性灰度插值
inline double getBilinearInterpolatedValue(const Mat &img, const Vector2d &pt) {
    uchar *d = &img.data[int(pt(1, 0)) * img.step + int(pt(0, 0))];
    double xx = pt(0, 0) - floor(pt(0, 0));
    double yy = pt(1, 0) - floor(pt(1, 0));
    return ((1 - xx) * (1 - yy) * double(d[0]) +
            xx * (1 - yy) * double(d[1]) +
            (1 - xx) * yy * double(d[img.step]) +
            xx * yy * double(d[img.step + 1])) / 255.0;
}

// ------------------------------------------------------------------
// 一些小工具
// 显示估计的深度图
// void plotDepth(const Mat &depth_truth, const Mat &depth_estimate);

// 像素到相机坐标系
inline Vector3d px2cam(const Vector2d px) {
    return Vector3d(
        (px(0, 0) - cx) / fx,
        (px(1, 0) - cy) / fy,
        1
    );
}

// 相机坐标系到像素
inline Vector2d cam2px(const Vector3d p_cam) {
    return Vector2d(
        p_cam(0, 0) * fx / p_cam(2, 0) + cx,
        p_cam(1, 0) * fy / p_cam(2, 0) + cy
    );
}

// 检测一个点是否在图像边框内
inline bool inside(const Vector2d &pt) {
    return pt(0, 0) >= boarder && pt(1, 0) >= boarder
           && pt(0, 0) + boarder < width && pt(1, 0) + boarder <= height;
}

// // 显示极线匹配
// void showEpipolarMatch(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_curr);

// // 显示极线
// void showEpipolarLine(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_min_curr,
//                       const Vector2d &px_max_curr);

/// 评测深度估计
void evaludateDepth(const Mat &depth_truth, const Mat &depth_estimate);
// ------------------------------------------------------------------

int main(int argc, char **argv) {
    if (argc != 2) {
        cout << "Usage: dense_mapping path_to_test_dataset" << endl;
        return -1;
    }

    // 从数据集读取数据
    vector<string> color_image_files;
    vector<SE3d> poses_TWC;
    Mat ref_depth;
    bool ret = readDatasetFiles(argv[1], color_image_files, poses_TWC, ref_depth);
    if (ret == false) {
        cout << "Reading image files failed!" << endl;
        return -1;
    }
    cout << "read total " << color_image_files.size() << " files." << endl;

    // 第一张图
    Mat ref = imread(color_image_files[0], 0);                // gray-scale image
    SE3d pose_ref_TWC = poses_TWC[0];
    double init_depth = 3.0;    // 深度初始值
    double init_cov2 = 3.0;     // 方差初始值
    Mat depth(height, width, CV_64F, init_depth);             // 深度图
    Mat depth_cov2(height, width, CV_64F, init_cov2);         // 深度图方差

    for (int index = 1; index < color_image_files.size(); index++) {
        cout << "*** loop " << index << " ***" << endl;
        Mat curr = imread(color_image_files[index], 0);
        if (curr.data == nullptr) continue;
        SE3d pose_curr_TWC = poses_TWC[index];
        SE3d pose_T_C_R = pose_curr_TWC.inverse() * pose_ref_TWC;   // 坐标转换关系： T_C_W * T_W_R = T_C_R
        update(ref, curr, pose_T_C_R, depth, depth_cov2);
        evaludateDepth(ref_depth, depth);
        std::cout << "after eval depth " << std::endl;
        plotDepth(ref_depth, depth);
        // imshow("image", curr);
        // waitKey(1);
        plotCur(curr);
    }

    cout << "estimation returns, saving depth map ..." << endl;
    imwrite("depth.png", depth);
    cout << "done." << endl;

    debug.close();
    return 0;
}

bool readDatasetFiles(
    const string &path,
    vector<string> &color_image_files,
    std::vector<SE3d> &poses,
    cv::Mat &ref_depth) {
    ifstream fin(path + "/first_200_frames_traj_over_table_input_sequence.txt");
    if (!fin) return false;

    while (!fin.eof()) {
        // 数据格式：图像文件名 tx, ty, tz, qx, qy, qz, qw ，注意是 TWC 而非 TCW
        string image;
        fin >> image;
        double data[7];
        for (double &d:data) fin >> d;

        color_image_files.push_back(path + string("/images/") + image);
        poses.push_back(
            SE3d(Quaterniond(data[6], data[3], data[4], data[5]),
                 Vector3d(data[0], data[1], data[2]))
        );
        if (!fin.good()) break;
    }
    fin.close();

    // load reference depth
    fin.open(path + "/depthmaps/scene_000.depth");
    ref_depth = cv::Mat(height, width, CV_64F);
    if (!fin) return false;
    for (int y = 0; y < height; y++)
        for (int x = 0; x < width; x++) {
            double depth = 0;
            fin >> depth;
            ref_depth.ptr<double>(y)[x] = depth / 100.0;
        }

    return true;
}

// 对整个深度图进行更新
void update(const Mat &ref, const Mat &curr, const SE3d &T_C_R, Mat &depth, Mat &depth_cov2) {
    std::cout << curr.size() << "\n";
    for (int x = boarder; x < width - boarder; x++)
    {
        for (int y = boarder; y < height - boarder; y++) {
            // 遍历每个像素
            // std::cout << depth.size() << " " << depth_cov2.size() << "\n";
            if (depth_cov2.ptr<double>(y)[x] < min_cov || depth_cov2.ptr<double>(y)[x] > max_cov) // 深度已收敛或发散
                continue;
            // 在极线上搜索 (x,y) 的匹配
            Vector2d pt_curr;
            Vector2d epipolar_direction;
            bool ret = epipolarSearch(
                ref,
                curr,
                T_C_R,
                Vector2d(x, y),
                depth.ptr<double>(y)[x],
                sqrt(depth_cov2.ptr<double>(y)[x]),
                pt_curr,
                epipolar_direction
            );

            if (ret == false) // 匹配失败
                continue;

            // debug << epipolar_direction.transpose() << "\n";

            // 取消该注释以显示匹配
            // showEpipolarMatch(ref, curr, Vector2d(x, y), pt_curr);

            // 匹配成功，更新深度图
            updateDepthFilter(Vector2d(x, y), pt_curr, T_C_R, epipolar_direction, depth, depth_cov2);
        }
    }
}

// 极线搜索
// 方法见书 12.2 12.3 两节
bool epipolarSearch(
    const Mat &ref, const Mat &curr,
    const SE3d &T_C_R, const Vector2d &pt_ref,
    const double &depth_mu, const double &depth_cov,
    Vector2d &pt_curr, Vector2d &epipolar_direction) {
    Vector3d f_ref = px2cam(pt_ref);
    f_ref.normalize();
    Vector3d P_ref = f_ref * depth_mu;    // 参考帧的 P 向量

    Vector2d px_mean_curr = cam2px(T_C_R * P_ref); // 按深度均值投影的像素
    double d_min = depth_mu - 3 * depth_cov, d_max = depth_mu + 3 * depth_cov;
    if (d_min < 0.1) d_min = 0.1;
    Vector2d px_min_curr = cam2px(T_C_R * (f_ref * d_min));    // 按最小深度投影的像素
    Vector2d px_max_curr = cam2px(T_C_R * (f_ref * d_max));    // 按最大深度投影的像素

    Vector2d epipolar_line = px_max_curr - px_min_curr;    // 极线（线段形式）
    epipolar_direction = epipolar_line;        // 极线方向
    epipolar_direction.normalize();
    double half_length = 0.5 * epipolar_line.norm();    // 极线线段的半长度
    if (half_length > 100) half_length = 100;   // 我们不希望搜索太多东西

    // 取消此句注释以显示极线（线段）
    // showEpipolarLine( ref, curr, pt_ref, px_min_curr, px_max_curr );

    // 在极线上搜索，以深度均值点为中心，左右各取半长度
    double best_ncc = -1.0;
    Vector2d best_px_curr;
    for (double l = -half_length; l <= half_length; l += 0.7) { // l+=sqrt(2)
        Vector2d px_curr = px_mean_curr + l * epipolar_direction;  // 待匹配点
        if (!inside(px_curr))
            continue;
        // 计算待匹配点与参考帧的 NCC
        double ncc = NCC(ref, curr, pt_ref, px_curr);
        if (ncc > best_ncc) {
            best_ncc = ncc;
            best_px_curr = px_curr;
        }
    }
    if (best_ncc < 0.85f)      // 只相信 NCC 很高的匹配
        return false;
    pt_curr = best_px_curr;
    return true;
}

double NCC(
    const Mat &ref, const Mat &curr,
    const Vector2d &pt_ref, const Vector2d &pt_curr) {
    // 零均值-归一化互相关
    // 先算均值
    double mean_ref = 0, mean_curr = 0;
    vector<double> values_ref, values_curr; // 参考帧和当前帧的均值
    for (int x = -ncc_window_size; x <= ncc_window_size; x++)
        for (int y = -ncc_window_size; y <= ncc_window_size; y++) {
            double value_ref = double(ref.ptr<uchar>(int(y + pt_ref(1, 0)))[int(x + pt_ref(0, 0))]) / 255.0;
            mean_ref += value_ref;

            double value_curr = getBilinearInterpolatedValue(curr, pt_curr + Vector2d(x, y));
            mean_curr += value_curr;

            values_ref.push_back(value_ref);
            values_curr.push_back(value_curr);
        }

    mean_ref /= ncc_area;
    mean_curr /= ncc_area;

    // 计算 Zero mean NCC
    double numerator = 0, demoniator1 = 0, demoniator2 = 0;
    for (int i = 0; i < values_ref.size(); i++) {
        double n = (values_ref[i] - mean_ref) * (values_curr[i] - mean_curr);
        numerator += n;
        demoniator1 += (values_ref[i] - mean_ref) * (values_ref[i] - mean_ref);
        demoniator2 += (values_curr[i] - mean_curr) * (values_curr[i] - mean_curr);
    }
    return numerator / sqrt(demoniator1 * demoniator2 + 1e-10);   // 防止分母出现零
}

bool updateDepthFilter(
    const Vector2d &pt_ref,
    const Vector2d &pt_curr,
    const SE3d &T_C_R,
    const Vector2d &epipolar_direction,
    Mat &depth,
    Mat &depth_cov2) {
    // 不知道这段还有没有人看
    // 用三角化计算深度
    SE3d T_R_C = T_C_R.inverse();
    Vector3d f_ref = px2cam(pt_ref);
    f_ref.normalize();
    Vector3d f_curr = px2cam(pt_curr);
    f_curr.normalize();

    // 方程
    // d_ref * f_ref = d_cur * ( R_RC * f_cur ) + t_RC
    // f2 = R_RC * f_cur
    // 转化成下面这个矩阵方程组
    // => [ f_ref^T f_ref, -f_ref^T f2 ] [d_ref]   [f_ref^T t]
    //    [ f_2^T f_ref, -f2^T f2      ] [d_cur] = [f2^T t   ]
    Vector3d t = T_R_C.translation();
    Vector3d f2 = T_R_C.so3() * f_curr;
    Vector2d b = Vector2d(t.dot(f_ref), t.dot(f2));
    Matrix2d A;
    A(0, 0) = f_ref.dot(f_ref);
    A(0, 1) = -f_ref.dot(f2);
    A(1, 0) = -A(0, 1);
    A(1, 1) = -f2.dot(f2);
    Vector2d ans = A.inverse() * b;
    Vector3d xm = ans[0] * f_ref;           // ref 侧的结果
    Vector3d xn = t + ans[1] * f2;          // cur 结果
    Vector3d p_esti = (xm + xn) / 2.0;      // P的位置，取两者的平均
    double depth_estimation = p_esti.norm();   // 深度值
    // debug << depth_estimation << "\n";

    // 计算不确定性（以一个像素为误差）
    Vector3d p = f_ref * depth_estimation;
    Vector3d a = p - t;
    double t_norm = t.norm();
    double a_norm = a.norm();
    double alpha = acos(f_ref.dot(t) / t_norm);
    double beta = acos(-a.dot(t) / (a_norm * t_norm));
    // debug << alpha << " " << beta << "\n";
    Vector3d f_curr_prime = px2cam(pt_curr + epipolar_direction);
    f_curr_prime.normalize();
    double beta_prime = acos(f_curr_prime.dot(-t) / t_norm);
    // debug << beta_prime << "\n";
    double gamma = M_PI - alpha - beta_prime;
    double p_prime = t_norm * sin(beta_prime) / sin(gamma);
    double d_cov = p_prime - depth_estimation;
    double d_cov2 = d_cov * d_cov;
    // debug << d_cov2  <<"\n";

    // 高斯融合
    double mu = depth.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))];
    double sigma2 = depth_cov2.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))];

    double mu_fuse = (d_cov2 * mu + sigma2 * depth_estimation) / (sigma2 + d_cov2);
    double sigma_fuse2 = (sigma2 * d_cov2) / (sigma2 + d_cov2);

    // debug << mu_fuse << "\n";
    depth.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = mu_fuse;
    depth_cov2.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = sigma_fuse2;

    return true;
}

// // 后面这些太简单我就不注释了（其实是因为懒）
// void plotDepth(const Mat &depth_truth, const Mat &depth_estimate) {
//     imshow("depth_truth", depth_truth * 0.4);
//     imshow("depth_estimate", depth_estimate * 0.4);
//     imshow("depth_error", depth_truth - depth_estimate);
//     waitKey(1);
// }

void evaludateDepth(const Mat &depth_truth, const Mat &depth_estimate) {
    double ave_depth_error = 0;     // 平均误差
    double ave_depth_error_sq = 0;      // 平方误差
    int cnt_depth_data = 0;
    for (int y = boarder; y < depth_truth.rows - boarder; y++)
    {
        for (int x = boarder; x < depth_truth.cols - boarder; x++) {
            double error = depth_truth.at<double>(y, x) - depth_estimate.at<double>(y, x);
            ave_depth_error += error;
            ave_depth_error_sq += error * error;
            cnt_depth_data++;
        }
    }
    ave_depth_error /= cnt_depth_data;
    ave_depth_error_sq /= cnt_depth_data;

    cout << "Average squared error = " << ave_depth_error_sq << ", average error: " << ave_depth_error << endl;
}

// void showEpipolarMatch(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_curr) {
//     Mat ref_show, curr_show;
//     cv::cvtColor(ref, ref_show, CV_GRAY2BGR);
//     cv::cvtColor(curr, curr_show, CV_GRAY2BGR);

//     cv::circle(ref_show, cv::Point2f(px_ref(0, 0), px_ref(1, 0)), 5, cv::Scalar(0, 0, 250), 2);
//     cv::circle(curr_show, cv::Point2f(px_curr(0, 0), px_curr(1, 0)), 5, cv::Scalar(0, 0, 250), 2);

//     imshow("ref", ref_show);
//     imshow("curr", curr_show);
//     waitKey(1);
// }

// void showEpipolarLine(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_min_curr,
//                       const Vector2d &px_max_curr) {

//     Mat ref_show, curr_show;
//     cv::cvtColor(ref, ref_show, CV_GRAY2BGR);
//     cv::cvtColor(curr, curr_show, CV_GRAY2BGR);

//     cv::circle(ref_show, cv::Point2f(px_ref(0, 0), px_ref(1, 0)), 5, cv::Scalar(0, 255, 0), 2);
//     cv::circle(curr_show, cv::Point2f(px_min_curr(0, 0), px_min_curr(1, 0)), 5, cv::Scalar(0, 255, 0), 2);
//     cv::circle(curr_show, cv::Point2f(px_max_curr(0, 0), px_max_curr(1, 0)), 5, cv::Scalar(0, 255, 0), 2);
//     cv::line(curr_show, Point2f(px_min_curr(0, 0), px_min_curr(1, 0)), Point2f(px_max_curr(0, 0), px_max_curr(1, 0)),
//              Scalar(0, 255, 0), 1);

//     imshow("ref", ref_show);
//     imshow("curr", curr_show);
//     waitKey(1);
// }


#include <Eigen/Core>
#include <Eigen/Dense>
#include <vector>
#include <fstream>
#include <iostream>
#include <sophus/se3.h>

using namespace Eigen;
using namespace std;

typedef vector<Vector3d,Eigen::aligned_allocator<Vector3d> > VecVector3d;
typedef vector<Vector2d,Eigen::aligned_allocator<Vector2d> > VecVector2d;

typedef Matrix<double ,6,1> Vector6d;
string p3d_file="../p3d.txt";
string p2d_file="../p2d.txt";

int main(int argc ,char** argv)
{
    VecVector2d p2d;
    VecVector3d p3d;
    Matrix3d k; //内参
    double fx=520.9,fy=521.0,cx=325.1,cy=249.7;
    k<<fx,0,cx,0,fy,cy,0,0,1;

    //load points into p3d and p2d
    //START YOUR CODE HERE
    ifstream file1(p3d_file);
    if(!file1.is_open())
    {
        perror("p3d open failed");
    }
    ifstream file2(p2d_file);
    if(!file2.is_open())
    {
        perror("p2d open failed");
    }
    while (!file1.eof())
    {
        double p3[3] = {0};
        for (auto &p:p3)
        {
            file1 >> p;
        }
        p3d.push_back(Vector3d(p3[0], p3[1], p3[2]));
    }
    while (!file2.eof())
    {
        double p2[2]={0};
        for (auto& p:p2)
        {
            file2>>p;
        }
        Vector2d v(p2[0],p2[1]);
        p2d.push_back(v);
        //cout<<"p2d "<<v<<" p3d "<<Vector3d(p3[0],p3[1],p3[2])<<endl;
    }
    //END YOUR CODE HERE
    assert( p3d.size() == p2d.size() ); //如果二者行数相等，则而已继续执行，否则程序停止运行

    int iterations=100;
    double cost=0,lastcost=0;
    int nPoints=p3d.size();
    cout<<"points: "<<nPoints<<endl;

    Matrix3d I = Matrix3d::Identity(); //声明单位矩阵
    Vector3d t ;
    t.setZero();
    //cout<< "I:\n"<<I<<endl;
    //cout<< "t:\n"<<t<<endl;
    Sophus::SE3 T_esti(I,t);//变换矩阵
    //Sophus::SE3 T_esti;
    cout<<"T_esti:\n"<<T_esti.matrix()<<endl;

   for (int iter = 0; iter <iterations ; ++iter) //迭代100次
    {
        Matrix<double,6,6> H = Matrix<double,6,6>::Zero();
        Vector6d b = Vector6d::Zero();

        cost=0;
        //compute cost
        for (int i = 0; i <p3d.size() ; ++i) //遍历数组
        {
            //compute cost for p3d[i] and p2d[i]
            //START YOUR CODE HERE
            Vector2d ui=p2d[i]; //2*1 p2p点
            //Vector3d pi=p3d[i];
            //（7.34）4*4 的变换矩阵 × 4*1的3d点

            Vector4d pii = T_esti.matrix()*Vector4d(p3d[i][0],p3d[i][1],p3d[i][2],1);

           // cout<< "pii:\n"<< pii <<endl;
           // cout<< "k:\n" << k << endl;
            // 3*1的s*u =3*3 的内参 × 3*1的(SE3*P)
            Vector3d pi=k*Vector3d(pii[0],pii[1],pii[2]);//s*u

            cout<< "pi:\n"<< pi<<endl;

            //求误差
            Vector2d e( ui[0]-pi[0]/pi[2] , ui[1]-pi[1]/pi[2] ); //除以pi[2]是归一化过程，误差为2*1
            cost += e(0,0)*e(0,0)+e(1,0)*e(1,0);//e.transpose()*e;
            //END YOUR CODE  HERE

            //compute jacobian
            Matrix<double,2,6> J; //定义2*6 雅克比

            //START YOUR CODE HERE
            //7.45
            J(0,0)=-fx/pii[2];
            J(0,1)=0;
            J(0,2)=fx*pii[0]/(pii[2]*pii[2]);
            J(0,3)=fx*pii[0]*pii[1]/(pii[2]*pii[2]);
            J(0,4)=-fx-fx*pii[0]*pii[0]/(pii[2]*pii[2]);
            J(0,5)=fx*pii[1]/pii[2];

            J(1,0)=0;
            J(1,1)=-fy/pii[2];
            J(1,2)=fy*pii[1]/(pii[2]*pii[2]);
            J(1,3)=fy+fy*pii[2]*pii[2]/(pii[0]*pii[0]);
            J(1,4)=-fy*pii[0]*pii[1]/(pii[2]*pii[2]);
            J(1,5)=-fy*pii[0]/pii[2];
            //END YOUR CODE HERE

            H+=J.transpose()*J;
            b+=-J.transpose()*e;
        }

        //solve dx
        Vector6d dx;

        //START YOUR CODE HERE
        dx=H.ldlt().solve(b);
        //END YOUR CODE HERE

        cout<<"iteration "<<iter<<" cost="<<cout.precision(12)<<cost<<endl;
        if(isnan(dx[0]))
        {
            cout<<"result is nan!"<<endl;
            break;
        }

        if(iter>0&&cost>=lastcost)
        {
            //cost increase,update is not good
            cout<<"cost: "<<cout.precision(12)<<cost<<", last cost: "<<cout.precision(12)<<lastcost<<endl;
            break;
        }

        //update your estimation
        //START YOUR CODE HERE
        T_esti=Sophus::SE3::exp(dx) * T_esti;

        //END YOUR CODE HERE
        lastcost=cost;
        cout<<"iteration "<<iter<<" cost="<<cout.precision(12)<<cost<<endl;
    }

    cout<<"estimated pose: \n"<<T_esti.matrix()<<endl;

    return 0;
}


/*
 * geometry.cpp
 * Copyright (C) 2018 exbot <exbot@ubuntu>
 *
 * Distributed under terms of the MIT license.
 */

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

using namespace std;
using namespace Eigen;

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

		Isometry3d T_cw1 = Isometry3d::Identity();
		Isometry3d T_cw2 = Isometry3d::Identity();

		Quaterniond q_cw1(0.55, 0.3, 0.2, 0.2);
		Quaterniond q_cw2(-0.1, 0.3, -0.7, 0.2);

		Vector3d t_cw2(-0.1,0.4,0.8);
		Vector3d t_cw1(0.7,1.1,0.2);
		Vector3d p_c1(0.5,-0.1,0.2);

		T_cw1.rotate(q_cw1.normalized().toRotationMatrix());
		T_cw1.pretranslate(t_cw1);
		T_cw2.rotate(q_cw2.normalized().toRotationMatrix());
		T_cw2.pretranslate(t_cw2);

		Vector3d p_c2 = T_cw2 * T_cw1.inverse() * p_c1;
		cout << "P in camera2: "<< endl << p_c2 << endl;
return 1;
}


#include <armadillo>
#include "mcpdft.h"
#include "functional.h"

namespace mcpdft {
   /*================================================================*/
   /*                      Exchange Functionals                      */
   /*================================================================*/
   Functional::Functional()  {}
   Functional::~Functional() {}

   double Functional::EX_LSDA(const MCPDFT *mc,
                              const arma::vec &rho_a,
                              const arma::vec &rho_b) {
       const double alpha = (2.0/3.0);      // Slater value: a constant
       const double Cx = (9.0/8.0) * alpha * pow(3.0/M_PI,1.0/3.0);

       size_t npts = mc->get_npts();
       arma::vec W(mc->get_w());

       double exc = 0.0;
       for (size_t p = 0; p < npts; p++) {
           double exa = pow(2.0,1.0/3.0) * Cx * pow( rho_a(p), 4.0/3.0) ;
           double exb = pow(2.0,1.0/3.0) * Cx * pow( rho_b(p), 4.0/3.0) ;
           double ex_LSDA = exa + exb;
           exc += - ex_LSDA * W(p);
       }
       return exc;
   }

   double Functional::EX_PBE(const MCPDFT *mc,
		             const arma::vec &rho_a,
		             const arma::vec &rho_b,
		             const arma::vec &sigma_aa,
		             const arma::vec &sigma_bb) {

       const double delta = 0.06672455060314922;
       const double MU = (1.0/3.0) * delta * M_PI * M_PI;
       const double KAPPA = 0.804;
       double tol = 1.0e-20;

       auto kF = [](double RHO) -> double {
                 double dum = pow(3.0 * M_PI * M_PI * RHO ,1.0/3.0);
                 return dum;
       };

       auto eX = [=](double RHO) -> double {
                 double temp = -(3.0 * kF(RHO)) / (4.0 * M_PI);
                 return temp;
       };

       auto FX = [=](double SIGMA) -> double {
                 double temp = 1.0 + KAPPA - KAPPA * pow( (1.0 + (MU * pow(SIGMA,2.0)) / KAPPA ), -1.0 );
                 return temp;
       };

       auto S = [=](double RHO, double SIGMA) -> double {
                double temp = sqrt(SIGMA) / (2.0 * kF(RHO) * RHO);
                return temp;
       };

       size_t npts = mc->get_npts();
       arma::vec W(mc->get_w());

       double exc = 0.0;
       for (int p = 0; p < npts; p++) {
           double rhoa = rho_a(p);
           double rhob = rho_b(p);
           double rho = rhoa + rhob;
           double sigmaaa = sigma_aa(p);
           double sigmabb = sigma_bb(p);
           double sigma = 0.0;
           if ( rho > tol ) {
              if ( rhoa < tol ){
                 rho = rhob;
                 sigmabb = std::max(0.0,sigmabb);
                 sigma = sigmabb;

                 double zk = eX(2.0 * rho) * FX(S(2.0 * rho, 4.0 * sigma));
                 exc += rho * zk * W(p);
              }else if ( rhob < tol ){
                       rho = rhoa;
                       sigmaaa = std::max(0.0,sigmaaa);
                       sigma = sigmaaa;
                       double zk = eX(2.0 * rho) * FX(S(2.0 * rho, 4.0 * sigma));
                       exc += rho * zk * W(p);
              }else {
                    double zka = rhoa * eX(2.0 * rhoa) * FX(S(2.0 * rhoa, 4.0 * sigmaaa));
                    double zkb = rhob * eX(2.0 * rhob) * FX(S(2.0 * rhob, 4.0 * sigmabb));
                    double zk = zka + zkb;
                    exc += zk * W(p);
              }
           }else{
                   exc += 0.0;
                }
       }
       return exc;
   }
   /*================================================================*/
   /*                    Correlation Functionals                     */
   /*================================================================*/
    double Functional::EC_VWN3(const MCPDFT *mc,
                               const arma::vec &rho_a,
                               const arma::vec &rho_b) {
       double tol = 1.0e-20;

       const double ecp1 = 0.03109070000;
       const double ecp2 = -0.409286;
       const double ecp3 = 13.0720;
       const double ecp4 = 42.7198;
       const double ecf1 = 0.01554535000;
       const double ecf2 = -0.743294;
       const double ecf3 = 20.1231;
       const double ecf4 = 101.578;
       const double d2Fz = 1.7099209341613656173;

       size_t npts = mc->get_npts();
       arma::vec W(mc->get_w());

       auto x = [](double RHO) -> double {
                double rs = pow( 3.0 / ( 4.0 * M_PI * RHO ) , 1.0/3.0 );
                double dum = sqrt(rs);
                return dum;
       };
       auto Fz = [](double ZETA) -> double {
                 double dum = (pow((1.0 + ZETA) ,4.0/3.0) + pow((1.0 - ZETA) ,4.0/3.0) - 2.0) / (2.0 * pow(2.0,1.0/3.0) - 2.0);
                 return dum;
       };
       auto X = [](double i, double c, double d) -> double {
                double temp = pow(i,2.0) + c * i + d;
                return temp;
       };
       auto Q = [](double c, double d) -> double {
                double temp1 = sqrt( 4 * d - pow(c,2.0) );
                return temp1;
       };
       auto q = [=](double RHO, double A, double p, double c, double d) -> double {
                double dum1 = A * ( log( pow(x(RHO),2.0) / X(x(RHO),c,d) ) + 2.0 * c * atan( Q(c,d)/(2.0*x(RHO) + c) ) * pow(Q(c,d),-1.0)
                            - c * p * ( log( pow(x(RHO)-p,2.0) / X(x(RHO),c,d) ) + 2.0 * (c + 2.0 * p) * atan( Q(c,d)/(2.0*x(RHO) + c) )
                            * pow(Q(c,d),-1.0) ) * pow(X(p,c,d),-1.0) );
                return dum1;
       };
       auto EcP = [=](double RHO) -> double {
                  double dumm = q(RHO,ecp1,ecp2,ecp3,ecp4);
                  return dumm;
       };
       auto EcF = [=](double RHO) -> double {
                  double dum = q(RHO,ecf1,ecf2,ecf3,ecf4);
                  return dum;
       };

       double exc = 0.0;
       for (int p = 0; p < npts; p++) {
           double rhoa = rho_a(p);
           double rhob = rho_b(p);
           double rho = rhoa + rhob;
           double zeta = 0.0;
           if ( rho > tol ) {
              if ( rhoa < tol ){
                 rho = rhob;
                 zeta = 1.0;
              }else if ( rhob < tol ){
                       rho = rhoa;
                       zeta = 1.0;
              }else {/* if (!(rhoa < tol) && !(rhob < tol) ) */
                    zeta = (rhoa - rhob) / rho;
              }
              double zk = EcP(rho) + Fz(zeta) * (EcF(rho) - EcP(rho));
              exc += rho * zk * W(p);
           }else{
                   double zk = 0.0;
                   exc += 0.0;
                }
       }
       return exc;
   }

   double Functional::EC_PBE(const MCPDFT *mc,
		             const arma::vec &rho_a,
		             const arma::vec &rho_b,
                             const arma::vec &sigma_aa,
                             const arma::vec &sigma_ab,
			     const arma::vec &sigma_bb) {
      double tol = 1.0e-20;
      size_t npts = mc->get_npts();
      arma::vec W(mc->get_w());
      const double pa = 1.0;
      const double Aa = 0.0168869;
      const double a1a = 0.11125;
      const double b1a = 10.357;
      const double b2a = 3.6231;
      const double b3a = 0.88026;
      const double b4a = 0.49671;
      const double pe = 1.0;
      const double c0p = 0.0310907;
      const double a1p = 0.21370;
      const double b1p = 7.5957;
      const double b2p = 3.5876;
      const double b3p = 1.6382;
      const double b4p = 0.49294;
      const double c0f = 0.01554535;
      const double a1f = 0.20548;
      const double b1f = 14.1189;
      const double b2f = 6.1977;
      const double b3f = 3.3662;
      const double b4f = 0.62517;
      const double d2Fz = 1.7099209341613656173;
      const double BETA = 0.06672455060314922;
      const double GAMMA = 0.0310906908696549;

      auto Fi = [=](double ZETA) -> double {
                double dumm = 0.5 * (pow((1.0 + ZETA) ,2.0/3.0 ) + pow((1.0 - ZETA) ,2.0/3.0));
                return dumm;
      };

      auto kF = [](double RHO) -> double {
                double dum = pow(3.0 * M_PI * M_PI * RHO ,1.0/3.0);
                return dum;
      };

      auto ks = [=](double RHO) -> double {
                double temp = sqrt(4.0 * kF(RHO) / M_PI);
                return temp;
      };

      auto Fz = [](double ZETA) -> double {
                double dum = (pow((1.0 + ZETA) ,4.0/3.0) + pow((1.0 - ZETA) ,4.0/3.0) - 2.0) / (2.0 * pow(2.0,1.0/3.0) - 2.0);
                return dum;
      };

      auto t = [=](double RHO, double SIGMA, double ZETA) -> double {
               double temp = sqrt(SIGMA) / (2.0 * ks(RHO) * Fi(ZETA) * RHO);
               return temp;
      };

      auto G = [](double r, double T, double a1, double b1, double b2, double b3, double b4, double p) -> double {
               double dum = -2.0 * T * (1.0 + a1 * r) * log(1.0 + 0.5 * pow(T * (b1 * sqrt(r) + b2 * r + b3 * pow(r,3.0/2.0) + b4 * pow(r, p+1.0)) ,-1.0));
               return dum;

      };

      auto Ac = [=](double r) -> double {
                double temp = -G(r,Aa,a1a,b1a,b2a,b3a,b4a,pa);
                return temp;
      };

      auto EcP = [=](double r) -> double {
                 double dum = G(r,c0p,a1p,b1p,b2p,b3p,b4p,pe);
                 return dum;
      };

      auto EcF = [=](double r) -> double {
                 double dumm = G(r,c0f,a1f,b1f,b2f,b3f,b4f,pe);
                 return dumm;
      };

      auto Ec = [=](double r, double ZETA) -> double {
                double dum = EcP(r) + ( Ac(r) * Fz(ZETA) * (1.0 - pow(ZETA ,4.0)) ) / d2Fz + ( EcF(r) - EcP(r) ) * Fz(ZETA) * pow(ZETA ,4.0);
                return dum;
      };

      auto A = [=](double r, double ZETA) -> double {
               double dum = (BETA/GAMMA) * pow( exp(-Ec(r,ZETA) / (pow(Fi(ZETA),3.0) * GAMMA)) - 1.0, -1.0);
               return dum;
      };

      auto H = [=](double RHO, double SIGMA, double r, double ZETA) -> double {
               double temp = pow(Fi(ZETA),3.0) * GAMMA * log(1.0 + (BETA/GAMMA) * pow(t(RHO,SIGMA,ZETA) ,2.0) * (1.0 + A(r,ZETA) * pow(t(RHO,SIGMA,ZETA),2.0))
                           / (1.0 + A(r,ZETA) * pow(t(RHO,SIGMA,ZETA),2.0) + pow(A(r,ZETA),2.0) * pow(t(RHO,SIGMA,ZETA),4.0)));
               return temp;
      };

      double exc = 0.0;
      for (int p = 0; p < npts; p++) {
          double rhoa = rho_a(p);
          double rhob = rho_b(p);
          double rho = rhoa + rhob;
          double zeta = (rhoa - rhob) / rho;
          double rs =  pow( 3.0 / ( 4.0 * M_PI * rho) , 1.0/3.0 );
          double sigmaaa = sigma_aa(p);
          double sigmaab = sigma_ab(p);
          double sigmabb = sigma_bb(p);
          double sigma = sigmaaa + sigmabb + 2.0 * sigmaab;
          if ( rho > tol ) {
             if ( rhoa < tol ){
                rho = rhob;
                sigmabb = std::max(0.0,sigmabb);
                sigma = sigmabb;
                zeta = 1.0;
             }else if ( rhob < tol ){
                      rho = rhoa;
                      sigmaaa = std::max(0.0,sigmaaa);
                      sigma = sigmaaa;
                      zeta = 1.0;
             }else/* if (!(rhoa < tol) && !(rhob < tol) ) */{
                      sigmaaa = std::max(0.0,sigmaaa);
                      sigmabb = std::max(0.0,sigmabb);
                      sigma = sigmaaa + sigmabb + 2.0 * sigmaab;
             }
             double zk = H(rho,sigma,rs,zeta) + Ec(rs,zeta);
             exc += rho * zk * W(p);
          }else{
                  double zk = 0.0;
                  exc += rho * zk * W(p);
               }
      }
      return exc;
   }
}


//
// Created by krab1k on 02/01/19.
//

#include <functional>
#include <string>
#include <cmath>
#include <Eigen/LU>

#include "qeq.h"
#include "../structures/atom.h"
#include "../geometry.h"
#include "../parameters.h"

CHARGEFW2_METHOD(QEq)

double QEq::overlap_term(const Atom &atom_i, const Atom &atom_j, const std::string &type) const {
    auto Ji = parameters_->atom()->parameter(atom::hardness)(atom_i);
    auto Jj = parameters_->atom()->parameter(atom::hardness)(atom_j);
    auto Rij = distance(atom_i, atom_j);
    if (type == "Nishimoto-Mataga") {
        return 1 / (Rij + 2 / (Ji + Jj));
    } else if (type == "Nishimoto-Mataga-Weiss") {
        const double f = 1.2;
        return f / (Rij + (2 * f) / (Ji + Jj));
    } else if (type == "Ohno") {
        return 1 / std::sqrt(Rij * Rij + std::pow(2 / (Ji + Jj), 2));
    } else if (type == "Ohno-Klopman") {
        return 1 / std::sqrt(Rij * Rij + std::pow(1 / (2 * Ji) + 1 / (2 * Jj), 2));
    } else if (type == "DasGupta-Huzinaga") {
        const double k = 0.4;
        return 1 / (Rij + 1 / (Ji / 2 * exp(k * Rij) + Jj / 2 * exp(k * Rij)));
    } else /* (type == "Louwen-Vogt") */ {
        const double gamma = (Ji + Jj) / 2;
        return 1 / std::cbrt(1 / std::pow(gamma, 3) + std::pow(Rij, 3));
    }
}

Eigen::VectorXd QEq::EE_system(const std::vector<const Atom *> &atoms, double total_charge) const {

    size_t n = atoms.size();

    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(n + 1, n + 1);
    Eigen::VectorXd b = Eigen::VectorXd::Zero(n + 1);

    const auto type = get_option_value<std::string>("overlap_term");

    for (size_t i = 0; i < n; i++) {
        const auto &atom_i = *atoms[i];
        A(i, i) = parameters_->atom()->parameter(atom::hardness)(atom_i);
        b(i) = - parameters_->atom()->parameter(atom::electronegativity)(atom_i);
        for (size_t j = i + 1; j < n; j++) {
            const auto &atom_j = *atoms[j];
            auto x = overlap_term(atom_i, atom_j, type);
            A(i, j) = x;
            A(j, i) = x;
        }
    }

    A.row(n) = Eigen::VectorXd::Constant(n + 1, 1);
    A.col(n) = Eigen::VectorXd::Constant(n + 1, 1);
    A(n, n) = 0;
    b(n) = total_charge;

    return A.partialPivLu().solve(b).head(n);
}

std::vector<double> QEq::calculate_charges(const Molecule &molecule) const {
    auto f = [this](const std::vector<const Atom *> &atoms, double total_charge) -> Eigen::VectorXd {
        return EE_system(atoms, total_charge);
    };

    Eigen::VectorXd q = solve_EE(molecule, f);
    return std::vector<double>(q.data(), q.data() + q.size());
}


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

#include <cmath>
#include <algorithm>
// #include <numbers>

#include <armadillo>

#include <nav_msgs/Path.h>
#include <tf2/LinearMath/Quaternion.h>

#include <ergodic_exploration/collision.hpp>

namespace ergodic_exploration
{
using arma::mat;
using arma::vec;

// TODO: why gcc cant find numbers
constexpr double PI = 3.14159265358979323846;

/**
 * @brief approximately compare two floating-point numbers
 * @param d1 - a number to compare
 * @param d2 - a second number to compare
 * @param epsilon - absolute threshold required for equality
 * @return true if abs(d1 - d2) < epsilon
 */
inline bool almost_equal(double d1, double d2, double epsilon = 1.0e-12)
{
  return std::fabs(d1 - d2) < epsilon ? true : false;
}

/**
 * @brief Wraps angle between -pi and pi
 * @param rad - angle in radians
 * @return wrapped angle in radians
 */
inline double normalize_angle_PI(double rad)
{
  // floating point remainder essentially this is fmod
  const auto q = std::floor((rad + PI) / (2.0 * PI));
  rad = (rad + PI) - q * 2.0 * PI;

  if (rad < 0.0)
  {
    rad += 2.0 * PI;
  }

  return (rad - PI);
}

/**
 * @brief Wraps angle between 0 and 2pi or 0 to -2pi
 * @param rad - angle in radians
 * @return wrapped angle in radians
 */
inline double normalize_angle_2PI(double rad)
{
  // floating point remainder essentially this is fmod
  const auto q = std::floor(rad / (2.0 * PI));
  rad = (rad)-q * 2.0 * PI;

  if (rad < 0.0)
  {
    rad += 2.0 * PI;
  }

  return rad;
}

/**
 * @brief Get yaw from quaternion
 * @param qx - x-axis rotation component
 * @param qy - y-axis rotation component
 * @param qz - z-axis rotation component
 * @param qw - rotation magnitude
 * @return yaw (rad)
 */
inline double getYaw(double qx, double qy, double qz, double qw)
{
  // TODO: add unit test
  // Source: https://github.com/ros/geometry2/blob/noetic-devel/tf2/include/tf2/impl/utils.h#L122
  const auto sqx = qx * qx;
  const auto sqy = qy * qy;
  const auto sqz = qz * qz;
  const auto sqw = qw * qw;

  // Normalization added from urdfom_headers
  const auto sarg = -2.0 * (qx * qz - qw * qy) / (sqx + sqy + sqz + sqw);

  // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
  if (sarg < -0.99999 || almost_equal(sarg, -0.99999))
  {
    return -2.0 * std::atan2(qy, qx);
  }

  else if (sarg > 0.99999 || almost_equal(sarg, 0.99999))
  {
    return 2.0 * std::atan2(qy, qx);
  }

  return std::atan2(2.0 * (qx * qy + qw * qz), sqw + sqx - sqy - sqz);
}

/**
 * @brief Euclidean distance between two points
 * @param x0 - x-position point 0
 * @param y0 - y-position point 0
 * @param x1 - x-position point 1
 * @param y1 - y-position point 1
 * @return euclidean distance
 */
inline double distance(double x0, double y0, double x1, double y1)
{
  const auto dx = x1 - x0;
  const auto dy = y1 - y0;
  return std::sqrt(dx * dx + dy * dy);
}

/**
 * @brief Entropy of a single grid cell
 * @param p - probability grid cell is occupied represented as a decimal
 * @return entropy
 */
inline double entropy(double p)
{
  // Assign zero information gain
  if (almost_equal(0.0, p) || almost_equal(1.0, p) /*|| p < 0.0*/)
  {
    return 1e-3;
  }

  // unknowm: p = -1 => entropy(0.5) = 0.7
  else if (p < 0.0)
  {
    return 0.7;
  }

  return -p * std::log(p) - (1.0 - p) * std::log(1.0 - p);
}

/**
 * @brief Convert polar to cartesian coordinates
 * @param angle - angle in radians
 * @param range - range measurement
 */
inline vec polar2Cartesian(double angle, double range)
{
  const auto x = range * std::cos(angle);
  const auto y = range * std::sin(angle);
  return { x, y };
}

/**
 * @brief Convert polar to cartesian homogenous coordinates
 * @param angle - angle in radians
 * @param range - range measurement
 */
inline vec polar2CartesianHomo(double angle, double range)
{
  const auto x = range * std::cos(angle);
  const auto y = range * std::sin(angle);
  return { x, y, 1.0 };
}

/**
 * @brief Construct 2D transformation matrix
 * @param x - x position
 * @param y - y position
 * @param angle - yaw in radians
 * @details 2D transformation
 */
inline mat transform2d(double x, double y, double angle)
{
  const mat trans2d = { { std::cos(angle), -std::sin(angle), x },
                        { std::sin(angle), std::cos(angle), y },
                        { 0.0, 0.0, 1.0 } };

  return trans2d;
}

/**
 * @brief Construct 2D transformation matrix
 * @param x - x position
 * @param y - y position
 * @details 2D transformation
 */
inline mat transform2d(double x, double y)
{
  const mat trans2d = { { 1.0, 0.0, x }, { 0.0, 1.0, y }, { 0.0, 0.0, 1.0 } };

  return trans2d;
}

/**
 * @brief Construct 2D transformation
 * @param angle - yaw in radians
 * @details 2D transformation
 */
inline mat transform2d(double angle)
{
  const mat trans2d = { { std::cos(angle), -std::sin(angle), 0.0 },
                        { std::sin(angle), std::cos(angle), 0.0 },
                        { 0.0, 0.0, 1.0 } };
  return trans2d;
}

/**
 * @brief Construct 2D transformation inverse
 * @param trans2d - 2D transformation
 * @details 2D transformation inverse
 */
inline mat transform2dInv(const mat& trans2d)
{
  // R^T flip sign in sin
  const auto stheta = -trans2d(1, 0);
  const auto ctheta = trans2d(0, 0);
  const auto theta = std::atan2(stheta, ctheta);

  // p' = -R^T * p
  const auto x = -(ctheta * trans2d(0, 2) - stheta * trans2d(1, 2));
  const auto y = -(stheta * trans2d(0, 2) + ctheta * trans2d(1, 2));

  return transform2d(theta, x, y);
}

/**
 * @brief Integrate a constant twist
 * @param x - current state [x, y, theta]
 * @param vb - current twist [vx, vy, w]
 * @param dt - time step
 * @return new pose
 */
inline vec integrate_twist(const vec& x, const vec& u, double dt)
{
  // Eqn. 13.35 and 13.36 pg 471 Modern Robotics
  // displacement b to b' (dx, dy, dth)
  vec dqb(3);

  // no rotation
  if (almost_equal(u(2), 0.0))
  {
    dqb(0) = u(0) * dt;
    dqb(1) = u(1) * dt;
    dqb(2) = 0.0;
  }

  else
  {
    const vec vb = u * dt;
    dqb(0) = (vb(0) * std::sin(vb(2)) + vb(1) * (std::cos(vb(2)) - 1.0)) / vb(2);

    dqb(1) = (vb(1) * std::sin(vb(2)) + vb(0) * (1.0 - std::cos(vb(2)))) / vb(2);

    dqb(2) = vb(2);
  }

  return x + transform2d(x(2)) * dqb;
}

/**
 * @brief Determine if control will cause a collision
 * @param collision - collision detector
 * @param grid - grid map
 * @param x0 - initial state
 * @param u - twist [vx, vy, w]
 * @param dt - time step in integration
 * @param horizon - length of integration
 * @return true if the control is collision free
 * @details The control is assumed to be constant and a twist is
 * integrated for a fixed amout of time
 */
inline bool validate_control(const Collision& collision, const GridMap& grid,
                             const vec& x0, const vec& u, double dt, double horizon)
{
  vec x = x0;
  const auto steps = static_cast<unsigned int>(std::abs(horizon / dt));

  for (unsigned int i = 0; i < steps; i++)
  {
    x = integrate_twist(x, u, dt);
    x(2) = normalize_angle_PI(x(2));

    if (collision.collisionCheck(grid, x))
    {
      return false;
    }
  }

  return true;
}

/**
 * @brief Visualize path from following a constant twist
 * @param x0 - current state
 * @param u - twist [vx, vy, w]
 * @param dt - time step
 * @param horizon - control horizon
 * @return trajectory
 */
inline nav_msgs::Path constTwistPath(const std::string& map_frame_id, const vec& x0,
                                     const vec& u, double dt, double horizon)
{
  nav_msgs::Path path;
  path.header.frame_id = map_frame_id;

  const auto steps = static_cast<unsigned int>(std::abs(horizon / dt));
  path.poses.resize(steps);

  vec x = x0;
  for (unsigned int i = 0; i < steps; i++)
  {
    x = integrate_twist(x, u, dt);

    path.poses.at(i).pose.position.x = x(0);
    path.poses.at(i).pose.position.y = x(1);

    tf2::Quaternion quat;
    quat.setRPY(0.0, 0.0, normalize_angle_PI(x(2)));

    path.poses.at(i).pose.orientation.x = quat.x();
    path.poses.at(i).pose.orientation.y = quat.y();
    path.poses.at(i).pose.orientation.z = quat.z();
    path.poses.at(i).pose.orientation.w = quat.w();
  }

  return path;
}

}  // namespace ergodic_exploration
#endif


#ifndef VECTOR3D_HEADER_DEFINED
#define VECTOR3D_HEADER_DEFINED

//#include <boost/functional/hash.hpp>

const double EPS_DBL = 1e-12;

struct vector3d{
    double x;
    double y;
    double z;

    vector3d(double x_ = 0.0, double y_ = 0.0, double z_ = 0.0) : x(x_), y(y_), z(z_) {}

    double operator *(const vector3d& vec) const{
        return x * vec.x + y * vec.y + z * vec.z;
    }
    vector3d operator / (double d) const {
        return vector3d(x / d, y / d, z / d);
    }
    vector3d norm() const{
        return *this / sqrt(*this * *this);
    }
    double length() const{
        return sqrt(*this * *this);
    }
    double angle(const vector3d& vec) const{
        return acos((*this * vec)/((*this).length() * vec.length()));
    }
    double distxy(const vector3d& vec) const{
        return sqrt((x-vec.x)*(x-vec.x)+(y-vec.y)*(y-vec.y));
    }
    vector3d operator ^ (const vector3d& vec) const{
        return vector3d(y*vec.z-z*vec.y, z*vec.x-x*vec.z,x*vec.y-y*vec.x);
    }
    vector3d operator + (const vector3d& vec) const{
        return vector3d(x+vec.x,y+vec.y,z+vec.z);
    }
    vector3d operator - (const vector3d& vec) const{
        return vector3d(x-vec.x,y-vec.y,z-vec.z);
    }

    bool operator < (const vector3d& vec) const{
        if(x<vec.x){
            return true;
        }
        else if(((x-vec.x)<EPS_DBL) && (y<vec.y)){
            return true;
        }
        else{
            return false;
        }
    }
    bool operator == (const vector3d& vec) const{
        return (std::abs(x-vec.x)<EPS_DBL) && (std::abs(y-vec.y)<EPS_DBL);
    }
};

inline std::ostream& operator << (std::ostream& os, const vector3d& vec){
    os << "<" << vec.x << ", " << vec.y << ", " << vec.z << ">";
    return os;
}

struct hash {
    size_t operator() (const vector3d& vec) const {
 //       size_t seed = 0;
 //       boost::hash_combine(seed, vec.x);
 //       boost::hash_combine(seed, vec.y);
 //       return seed;
    return std::hash<int>() ((int)(vec.y/25.0))+(((int)(vec.x/25.0)) << 16);
        //        return (size_t)vec.y+((size_t)vec.x << 32);
    }
};

#endif // VECTOR3D_HEADER_DEFINED