/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2014 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Authors: Damien Lebrun-Grandie, Bruno Turcksin, 2014
 */

// @sect3{Include files}

// 像往常一样，第一个任务是包括这些著名的deal.II库文件和一些C++头文件的功能。

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

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

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

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

#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/sparse_direct.h>

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

#include <fstream>
#include <iostream>
#include <cmath>
#include <map>

// 这是唯一一个新的包含文件：它包括所有的Runge-Kutta方法。

#include <deal.II/base/time_stepping.h>

// 接下来的步骤与之前所有的教程程序一样。我们把所有的东西放到一个自己的命名空间中，然后把deal.II的类和函数导入其中。

namespace Step52
{
  using namespace dealii;
// @sect3{The Diffusion class}

// 下一块是主类的声明。这个类中的大部分函数并不新鲜，在以前的教程中已经解释过了。唯一有趣的函数是  evaluate_diffusion()  和  id_minus_tau_J_inverse(). evaluate_diffusion()  评估扩散方程，  $M^{-1}(f(t,y))$  ，在一个给定的时间和一个给定的  $y$  。  id_minus_tau_J_inverse()  在给定的时间和给定的 $\tau$ 和 $y$ 下，评估 $\left(I-\tau M^{-1} \frac{\partial f(t,y)}{\partial y}\right)^{-1}$ 或类似的 $\left(M-\tau \frac{\partial f}{\partial y}\right)^{-1} M$ 。当使用隐式方法时，就需要这个函数。

  class Diffusion
  {
  public:
    Diffusion();

    void run();

  private:
    void setup_system();

    void assemble_system();

    double get_source(const double time, const Point<2> &point) const;

    Vector<double> evaluate_diffusion(const double          time,
                                      const Vector<double> &y) const;

    Vector<double> id_minus_tau_J_inverse(const double          time,
                                          const double          tau,
                                          const Vector<double> &y);

    void output_results(const double                     time,
                        const unsigned int               time_step,
                        TimeStepping::runge_kutta_method method) const;

// 接下来的三个函数分别是显式方法、隐式方法和嵌入式显式方法的驱动。嵌入显式方法的驱动函数返回执行的步数，鉴于它只接受作为参数传递的时间步数作为提示，但内部计算了最佳时间步数本身。

    void explicit_method(const TimeStepping::runge_kutta_method method,
                         const unsigned int                     n_time_steps,
                         const double                           initial_time,
                         const double                           final_time);

    void implicit_method(const TimeStepping::runge_kutta_method method,
                         const unsigned int                     n_time_steps,
                         const double                           initial_time,
                         const double                           final_time);

    unsigned int
    embedded_explicit_method(const TimeStepping::runge_kutta_method method,
                             const unsigned int n_time_steps,
                             const double       initial_time,
                             const double       final_time);

    const unsigned int fe_degree;

    const double diffusion_coefficient;
    const double absorption_cross_section;

    Triangulation<2> triangulation;

    const FE_Q<2> fe;

    DoFHandler<2> dof_handler;

    AffineConstraints<double> constraint_matrix;

    SparsityPattern sparsity_pattern;

    SparseMatrix<double> system_matrix;
    SparseMatrix<double> mass_matrix;
    SparseMatrix<double> mass_minus_tau_Jacobian;

    SparseDirectUMFPACK inverse_mass_matrix;

    Vector<double> solution;
  };

// 我们选择二次方有限元，并初始化参数。

  Diffusion::Diffusion()
    : fe_degree(2)
    , diffusion_coefficient(1. / 30.)
    , absorption_cross_section(1.)
    , fe(fe_degree)
    , dof_handler(triangulation)
  {}

// 现在，我们创建约束矩阵和稀疏模式。然后，我们初始化这些矩阵和求解向量。

  void Diffusion::setup_system()
  {
    dof_handler.distribute_dofs(fe);

    VectorTools::interpolate_boundary_values(dof_handler,
                                             1,
                                             Functions::ZeroFunction<2>(),
                                             constraint_matrix);
    constraint_matrix.close();

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

    system_matrix.reinit(sparsity_pattern);
    mass_matrix.reinit(sparsity_pattern);
    mass_minus_tau_Jacobian.reinit(sparsity_pattern);
    solution.reinit(dof_handler.n_dofs());
  }

//  @sect4{Diffusion::assemble_system}  在这个函数中，我们计算  $-\int D \nabla b_i \cdot \nabla b_j d\boldsymbol{r} - \int \Sigma_a b_i b_j d\boldsymbol{r}$  和质量矩阵  $\int b_i b_j d\boldsymbol{r}$  。然后使用直接求解器对质量矩阵进行反演；然后 inverse_mass_matrix 变量将存储质量矩阵的反值，这样 $M^{-1}$ 就可以使用该对象的 vmult() 函数应用于一个矢量。在内部，UMFPACK并没有真正存储矩阵的逆，而是存储它的LU因子；应用矩阵的逆相当于用这两个因子做一次正解和一次逆解，这与应用矩阵的显式逆具有相同的复杂性）。

  void Diffusion::assemble_system()
  {
    system_matrix = 0.;
    mass_matrix   = 0.;

    const QGauss<2> quadrature_formula(fe_degree + 1);

    FEValues<2> fe_values(fe,
                          quadrature_formula,
                          update_values | update_gradients | 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);
    FullMatrix<double> cell_mass_matrix(dofs_per_cell, dofs_per_cell);

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

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

        fe_values.reinit(cell);

        for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
          for (unsigned int i = 0; i < dofs_per_cell; ++i)
            for (unsigned int j = 0; j < dofs_per_cell; ++j)
              {
                cell_matrix(i, j) +=
                  ((-diffusion_coefficient *                // (-D
                      fe_values.shape_grad(i, q_point) *    //  * grad phi_i
                      fe_values.shape_grad(j, q_point)      //  * grad phi_j
                    - absorption_cross_section *            //  -Sigma
                        fe_values.shape_value(i, q_point) * //  * phi_i
                        fe_values.shape_value(j, q_point))  //  * phi_j)
                   * fe_values.JxW(q_point));               // * dx
                cell_mass_matrix(i, j) += fe_values.shape_value(i, q_point) *
                                          fe_values.shape_value(j, q_point) *
                                          fe_values.JxW(q_point);
              }

        cell->get_dof_indices(local_dof_indices);

        constraint_matrix.distribute_local_to_global(cell_matrix,
                                                     local_dof_indices,
                                                     system_matrix);
        constraint_matrix.distribute_local_to_global(cell_mass_matrix,
                                                     local_dof_indices,
                                                     mass_matrix);
      }

    inverse_mass_matrix.initialize(mass_matrix);
  }

//  @sect4{Diffusion::get_source}

// 在这个函数中，计算出特定时间和特定点的方程的源项。

  double Diffusion::get_source(const double time, const Point<2> &point) const
  {
    const double intensity = 10.;
    const double frequency = numbers::PI / 10.;
    const double b         = 5.;
    const double x         = point(0);

    return intensity *
           (frequency * std::cos(frequency * time) * (b * x - x * x) +
            std::sin(frequency * time) *
              (absorption_cross_section * (b * x - x * x) +
               2. * diffusion_coefficient));
  }

//  @sect4{Diffusion::evaluate_diffusion}

// 接下来，我们在给定的时间  $t$  和给定的矢量  $y$  评价扩散方程的弱形式。换句话说，正如介绍中所述，我们评估  $M^{-1}(-{\cal D}y - {\cal A}y + {\cal S})$  。为此，我们必须将矩阵 $-{\cal D} - {\cal A}$ （之前计算并存储在变量 system_matrix 中）应用于 $y$ ，然后添加源项，我们像通常那样进行积分。(如果你想节省几行代码，或者想利用并行积分的优势，可以用 VectorTools::create_right_hand_side() 来进行积分。) 然后将结果乘以 $M^{-1}$  。

  Vector<double> Diffusion::evaluate_diffusion(const double          time,
                                               const Vector<double> &y) const
  {
    Vector<double> tmp(dof_handler.n_dofs());
    tmp = 0.;
    system_matrix.vmult(tmp, y);

    const QGauss<2> quadrature_formula(fe_degree + 1);

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

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

    Vector<double> cell_source(dofs_per_cell);

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

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

        fe_values.reinit(cell);

        for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
          {
            const double source =
              get_source(time, fe_values.quadrature_point(q_point));
            for (unsigned int i = 0; i < dofs_per_cell; ++i)
              cell_source(i) += fe_values.shape_value(i, q_point) * // phi_i(x)
                                source *                            // * S(x)
                                fe_values.JxW(q_point);             // * dx
          }

        cell->get_dof_indices(local_dof_indices);

        constraint_matrix.distribute_local_to_global(cell_source,
                                                     local_dof_indices,
                                                     tmp);
      }

    Vector<double> value(dof_handler.n_dofs());
    inverse_mass_matrix.vmult(value, tmp);

    return value;
  }
// @sect4{Diffusion::id_minus_tau_J_inverse}

// 我们计算  $\left(M-\tau \frac{\partial f}{\partial y}\right)^{-1} M$  。这要分几个步骤进行。

// - 计算  $M-\tau \frac{\partial f}{\partial y}$  。

// - 反转矩阵，得到  $\left(M-\tau \frac{\partial f} {\partial y}\right)^{-1}$  。

// --计算 $tmp=My$ 。

// --计算 $z=\left(M-\tau \frac{\partial f}{\partial y}\right)^{-1} tmp =  \left(M-\tau \frac{\partial f}{\partial y}\right)^{-1} My$ 。

// - 返回z。

  Vector<double> Diffusion::id_minus_tau_J_inverse(const double /*time*/,
                                                   const double          tau,
                                                   const Vector<double> &y)
  {
    SparseDirectUMFPACK inverse_mass_minus_tau_Jacobian;

    mass_minus_tau_Jacobian.copy_from(mass_matrix);
    mass_minus_tau_Jacobian.add(-tau, system_matrix);

    inverse_mass_minus_tau_Jacobian.initialize(mass_minus_tau_Jacobian);

    Vector<double> tmp(dof_handler.n_dofs());
    mass_matrix.vmult(tmp, y);

    Vector<double> result(y);
    inverse_mass_minus_tau_Jacobian.vmult(result, tmp);

    return result;
  }

//  @sect4{Diffusion::output_results}

// 下面的函数将解决方案以vtu文件的形式输出，并以时间步长和时间步长方法的名称为索引。当然，所有时间步长方法的（精确）结果应该是一样的，但这里的输出至少可以让我们对它们进行比较。

  void Diffusion::output_results(const double                     time,
                                 const unsigned int               time_step,
                                 TimeStepping::runge_kutta_method method) const
  {
    std::string method_name;

    switch (method)
      {
        case TimeStepping::FORWARD_EULER:
          {
            method_name = "forward_euler";
            break;
          }
        case TimeStepping::RK_THIRD_ORDER:
          {
            method_name = "rk3";
            break;
          }
        case TimeStepping::RK_CLASSIC_FOURTH_ORDER:
          {
            method_name = "rk4";
            break;
          }
        case TimeStepping::BACKWARD_EULER:
          {
            method_name = "backward_euler";
            break;
          }
        case TimeStepping::IMPLICIT_MIDPOINT:
          {
            method_name = "implicit_midpoint";
            break;
          }
        case TimeStepping::SDIRK_TWO_STAGES:
          {
            method_name = "sdirk";
            break;
          }
        case TimeStepping::HEUN_EULER:
          {
            method_name = "heun_euler";
            break;
          }
        case TimeStepping::BOGACKI_SHAMPINE:
          {
            method_name = "bocacki_shampine";
            break;
          }
        case TimeStepping::DOPRI:
          {
            method_name = "dopri";
            break;
          }
        case TimeStepping::FEHLBERG:
          {
            method_name = "fehlberg";
            break;
          }
        case TimeStepping::CASH_KARP:
          {
            method_name = "cash_karp";
            break;
          }
        default:
          {
            break;
          }
      }

    DataOut<2> data_out;

    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution, "solution");

    data_out.build_patches();

    data_out.set_flags(DataOutBase::VtkFlags(time, time_step));

    const std::string filename = "solution_" + method_name + "-" +
                                 Utilities::int_to_string(time_step, 3) +
                                 ".vtu";
    std::ofstream output(filename);
    data_out.write_vtu(output);

    static std::vector<std::pair<double, std::string>> times_and_names;

    static std::string method_name_prev = "";
    static std::string pvd_filename;
    if (method_name_prev != method_name)
      {
        times_and_names.clear();
        method_name_prev = method_name;
        pvd_filename     = "solution_" + method_name + ".pvd";
      }
    times_and_names.emplace_back(time, filename);
    std::ofstream pvd_output(pvd_filename);
    DataOutBase::write_pvd_record(pvd_output, times_and_names);
  }
// @sect4{Diffusion::explicit_method}

// 这个函数是所有显式方法的驱动。在顶部，它初始化了时间步长和解决方案（通过将其设置为零，然后确保边界值和悬挂节点约束得到尊重；当然，对于我们在这里使用的网格，悬挂节点约束实际上不是一个问题）。然后调用 evolve_one_time_step ，执行一个时间步骤。时间是通过DiscreteTime对象来存储和增加的。

// 对于显式方法， evolve_one_time_step 需要评估 $M^{-1}(f(t,y))$ ，也就是说，它需要 evaluate_diffusion  。因为 evaluate_diffusion 是一个成员函数，它需要被绑定到 this 。在每个进化步骤之后，我们再次应用正确的边界值和悬挂节点约束。

// 最后，每隔10个时间步骤就会输出解决方案。

  void Diffusion::explicit_method(const TimeStepping::runge_kutta_method method,
                                  const unsigned int n_time_steps,
                                  const double       initial_time,
                                  const double       final_time)
  {
    const double time_step =
      (final_time - initial_time) / static_cast<double>(n_time_steps);

    solution = 0.;
    constraint_matrix.distribute(solution);

    TimeStepping::ExplicitRungeKutta<Vector<double>> explicit_runge_kutta(
      method);
    output_results(initial_time, 0, method);
    DiscreteTime time(initial_time, final_time, time_step);
    while (time.is_at_end() == false)
      {
        explicit_runge_kutta.evolve_one_time_step(
          [this](const double time, const Vector<double> &y) {
            return this->evaluate_diffusion(time, y);
          },
          time.get_current_time(),
          time.get_next_step_size(),
          solution);
        time.advance_time();

        constraint_matrix.distribute(solution);

        if (time.get_step_number() % 10 == 0)
          output_results(time.get_current_time(),
                         time.get_step_number(),
                         method);
      }
  }

//  @sect4{Diffusion::implicit_method}  这个函数等同于 explicit_method ，但用于隐式方法。当使用隐式方法时，我们需要评估 $M^{-1}(f(t,y))$ 和 $\left(I-\tau M^{-1} \frac{\partial f(t,y)}{\partial y}\right)^{-1}$ ，为此我们使用之前介绍的两个成员函数。

  void Diffusion::implicit_method(const TimeStepping::runge_kutta_method method,
                                  const unsigned int n_time_steps,
                                  const double       initial_time,
                                  const double       final_time)
  {
    const double time_step =
      (final_time - initial_time) / static_cast<double>(n_time_steps);

    solution = 0.;
    constraint_matrix.distribute(solution);

    TimeStepping::ImplicitRungeKutta<Vector<double>> implicit_runge_kutta(
      method);
    output_results(initial_time, 0, method);
    DiscreteTime time(initial_time, final_time, time_step);
    while (time.is_at_end() == false)
      {
        implicit_runge_kutta.evolve_one_time_step(
          [this](const double time, const Vector<double> &y) {
            return this->evaluate_diffusion(time, y);
          },
          [this](const double time, const double tau, const Vector<double> &y) {
            return this->id_minus_tau_J_inverse(time, tau, y);
          },
          time.get_current_time(),
          time.get_next_step_size(),
          solution);
        time.advance_time();

        constraint_matrix.distribute(solution);

        if (time.get_step_number() % 10 == 0)
          output_results(time.get_current_time(),
                         time.get_step_number(),
                         method);
      }
  }

//  @sect4{Diffusion::embedded_explicit_method}  这个函数是嵌入式显式方法的驱动。它需要更多的参数。

// - coarsen_param：当误差低于阈值时，乘以当前时间步长的系数。

// - refine_param: 当误差高于阈值时，乘以当前时间步长的系数。

// - min_delta: 可接受的最小时间步长。

// - max_delta: 可接受的最大时间步长。

// - refine_tol：时间步长超过的阈值。

// - coarsen_tol：阈值，低于该阈值的时间步长将被粗化。

// 嵌入方法使用一个猜测的时间步长。如果使用这个时间步长的误差太大，时间步长将被缩小。如果误差低于阈值，则在下一个时间步长时将尝试更大的时间步长。  delta_t_guess 是由嵌入式方法产生的猜测的时间步长。总之，时间步长有可能以三种方式修改。

// - 在 TimeStepping::EmbeddedExplicitRungeKutta::evolve_one_time_step(). 内减少或增加时间步长。

// - 使用计算出的  delta_t_guess  。

// - 自动调整最后一个时间步长，以确保模拟在  final_time  处精确结束。这种调整是在DiscreteTime实例中处理的。

  unsigned int Diffusion::embedded_explicit_method(
    const TimeStepping::runge_kutta_method method,
    const unsigned int                     n_time_steps,
    const double                           initial_time,
    const double                           final_time)
  {
    const double time_step =
      (final_time - initial_time) / static_cast<double>(n_time_steps);
    const double coarsen_param = 1.2;
    const double refine_param  = 0.8;
    const double min_delta     = 1e-8;
    const double max_delta     = 10 * time_step;
    const double refine_tol    = 1e-1;
    const double coarsen_tol   = 1e-5;

    solution = 0.;
    constraint_matrix.distribute(solution);

    TimeStepping::EmbeddedExplicitRungeKutta<Vector<double>>
      embedded_explicit_runge_kutta(method,
                                    coarsen_param,
                                    refine_param,
                                    min_delta,
                                    max_delta,
                                    refine_tol,
                                    coarsen_tol);
    output_results(initial_time, 0, method);
    DiscreteTime time(initial_time, final_time, time_step);
    while (time.is_at_end() == false)
      {
        const double new_time =
          embedded_explicit_runge_kutta.evolve_one_time_step(
            [this](const double time, const Vector<double> &y) {
              return this->evaluate_diffusion(time, y);
            },
            time.get_current_time(),
            time.get_next_step_size(),
            solution);
        time.set_next_step_size(new_time - time.get_current_time());
        time.advance_time();

        constraint_matrix.distribute(solution);

        if (time.get_step_number() % 10 == 0)
          output_results(time.get_current_time(),
                         time.get_step_number(),
                         method);

        time.set_desired_next_step_size(
          embedded_explicit_runge_kutta.get_status().delta_t_guess);
      }

    return time.get_step_number();
  }

//  @sect4{Diffusion::run}

// 下面是该程序的主要功能。在顶部，我们创建网格（一个[0,5]x[0,5]的正方形）并对其进行四次细化，得到一个有16乘16单元的网格，共256个。 然后我们将边界指示器设置为1，用于边界中 $x=0$ 和 $x=5$ 的部分。

  void Diffusion::run()
  {
    GridGenerator::hyper_cube(triangulation, 0., 5.);
    triangulation.refine_global(4);

    for (const auto &cell : triangulation.active_cell_iterators())
      for (const auto &face : cell->face_iterators())
        if (face->at_boundary())
          {
            if ((face->center()[0] == 0.) || (face->center()[0] == 5.))
              face->set_boundary_id(1);
            else
              face->set_boundary_id(0);
          }

// 接下来，我们设置线性系统并为其填充内容，以便在整个时间步进过程中使用它们。

    setup_system();

    assemble_system();

// 最后，我们使用命名空间TimeStepping中实现的几种Runge-Kutta方法来解决扩散问题，每次都会在结束时输出误差。(正如介绍中所解释的，由于精确解在最后时间为零，所以误差等于数值解，只需取解向量的 $l_2$ 准则即可计算出来。)

    unsigned int       n_steps      = 0;
    const unsigned int n_time_steps = 200;
    const double       initial_time = 0.;
    const double       final_time   = 10.;

    std::cout << "Explicit methods:" << std::endl;
    explicit_method(TimeStepping::FORWARD_EULER,
                    n_time_steps,
                    initial_time,
                    final_time);
    std::cout << "   Forward Euler:            error=" << solution.l2_norm()
              << std::endl;

    explicit_method(TimeStepping::RK_THIRD_ORDER,
                    n_time_steps,
                    initial_time,
                    final_time);
    std::cout << "   Third order Runge-Kutta:  error=" << solution.l2_norm()
              << std::endl;

    explicit_method(TimeStepping::RK_CLASSIC_FOURTH_ORDER,
                    n_time_steps,
                    initial_time,
                    final_time);
    std::cout << "   Fourth order Runge-Kutta: error=" << solution.l2_norm()
              << std::endl;
    std::cout << std::endl;

    std::cout << "Implicit methods:" << std::endl;
    implicit_method(TimeStepping::BACKWARD_EULER,
                    n_time_steps,
                    initial_time,
                    final_time);
    std::cout << "   Backward Euler:           error=" << solution.l2_norm()
              << std::endl;

    implicit_method(TimeStepping::IMPLICIT_MIDPOINT,
                    n_time_steps,
                    initial_time,
                    final_time);
    std::cout << "   Implicit Midpoint:        error=" << solution.l2_norm()
              << std::endl;

    implicit_method(TimeStepping::CRANK_NICOLSON,
                    n_time_steps,
                    initial_time,
                    final_time);
    std::cout << "   Crank-Nicolson:           error=" << solution.l2_norm()
              << std::endl;

    implicit_method(TimeStepping::SDIRK_TWO_STAGES,
                    n_time_steps,
                    initial_time,
                    final_time);
    std::cout << "   SDIRK:                    error=" << solution.l2_norm()
              << std::endl;
    std::cout << std::endl;

    std::cout << "Embedded explicit methods:" << std::endl;
    n_steps = embedded_explicit_method(TimeStepping::HEUN_EULER,
                                       n_time_steps,
                                       initial_time,
                                       final_time);
    std::cout << "   Heun-Euler:               error=" << solution.l2_norm()
              << std::endl;
    std::cout << "                   steps performed=" << n_steps << std::endl;

    n_steps = embedded_explicit_method(TimeStepping::BOGACKI_SHAMPINE,
                                       n_time_steps,
                                       initial_time,
                                       final_time);
    std::cout << "   Bogacki-Shampine:         error=" << solution.l2_norm()
              << std::endl;
    std::cout << "                   steps performed=" << n_steps << std::endl;

    n_steps = embedded_explicit_method(TimeStepping::DOPRI,
                                       n_time_steps,
                                       initial_time,
                                       final_time);
    std::cout << "   Dopri:                    error=" << solution.l2_norm()
              << std::endl;
    std::cout << "                   steps performed=" << n_steps << std::endl;

    n_steps = embedded_explicit_method(TimeStepping::FEHLBERG,
                                       n_time_steps,
                                       initial_time,
                                       final_time);
    std::cout << "   Fehlberg:                 error=" << solution.l2_norm()
              << std::endl;
    std::cout << "                   steps performed=" << n_steps << std::endl;

    n_steps = embedded_explicit_method(TimeStepping::CASH_KARP,
                                       n_time_steps,
                                       initial_time,
                                       final_time);
    std::cout << "   Cash-Karp:                error=" << solution.l2_norm()
              << std::endl;
    std::cout << "                   steps performed=" << n_steps << std::endl;
  }
} // namespace Step52

//  @sect3{The main() function}

// 下面的 main 函数与前面的例子类似，不需要注释。

int main()
{
  try
    {
      Step52::Diffusion diffusion;
      diffusion.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 <iostream>
#include "BenchTimer.h"
#include <Eigen/Dense>
#include <map>
#include <vector>
#include <string>
#include <sstream>
using namespace Eigen;

std::map<std::string,Array<float,1,8,DontAlign|RowMajor> > results;
std::vector<std::string> labels;
std::vector<Array2i> sizes;

template<typename Solver,typename MatrixType>
EIGEN_DONT_INLINE
void compute_norm_equation(Solver &solver, const MatrixType &A) {
  if(A.rows()!=A.cols())
    solver.compute(A.transpose()*A);
  else
    solver.compute(A);
}

template<typename Solver,typename MatrixType>
EIGEN_DONT_INLINE
void compute(Solver &solver, const MatrixType &A) {
  solver.compute(A);
}

template<typename Scalar,int Size>
void bench(int id, int rows, int size = Size)
{
  typedef Matrix<Scalar,Dynamic,Size> Mat;
  typedef Matrix<Scalar,Dynamic,Dynamic> MatDyn;
  typedef Matrix<Scalar,Size,Size> MatSquare;
  Mat A(rows,size);
  A.setRandom();
  if(rows==size)
    A = A*A.adjoint();
  BenchTimer t_llt, t_ldlt, t_lu, t_fplu, t_qr, t_cpqr, t_cod, t_fpqr, t_jsvd, t_bdcsvd;

  int svd_opt = ComputeThinU|ComputeThinV;

  int tries = 5;
  int rep = 1000/size;
  if(rep==0) rep = 1;
//   rep = rep*rep;

  LLT<MatSquare> llt(size);
  LDLT<MatSquare> ldlt(size);
  PartialPivLU<MatSquare> lu(size);
  FullPivLU<MatSquare> fplu(size,size);
  HouseholderQR<Mat> qr(A.rows(),A.cols());
  ColPivHouseholderQR<Mat> cpqr(A.rows(),A.cols());
  CompleteOrthogonalDecomposition<Mat> cod(A.rows(),A.cols());
  FullPivHouseholderQR<Mat> fpqr(A.rows(),A.cols());
  JacobiSVD<MatDyn> jsvd(A.rows(),A.cols());
  BDCSVD<MatDyn> bdcsvd(A.rows(),A.cols());

  BENCH(t_llt, tries, rep, compute_norm_equation(llt,A));
  BENCH(t_ldlt, tries, rep, compute_norm_equation(ldlt,A));
  BENCH(t_lu, tries, rep, compute_norm_equation(lu,A));
  if(size<=1000)
    BENCH(t_fplu, tries, rep, compute_norm_equation(fplu,A));
  BENCH(t_qr, tries, rep, compute(qr,A));
  BENCH(t_cpqr, tries, rep, compute(cpqr,A));
  BENCH(t_cod, tries, rep, compute(cod,A));
  if(size*rows<=10000000)
    BENCH(t_fpqr, tries, rep, compute(fpqr,A));
  if(size<500) // JacobiSVD is really too slow for too large matrices
    BENCH(t_jsvd, tries, rep, jsvd.compute(A,svd_opt));
//   if(size*rows<=20000000)
    BENCH(t_bdcsvd, tries, rep, bdcsvd.compute(A,svd_opt));

  results["LLT"][id] = t_llt.best();
  results["LDLT"][id] = t_ldlt.best();
  results["PartialPivLU"][id] = t_lu.best();
  results["FullPivLU"][id] = t_fplu.best();
  results["HouseholderQR"][id] = t_qr.best();
  results["ColPivHouseholderQR"][id] = t_cpqr.best();
  results["CompleteOrthogonalDecomposition"][id] = t_cod.best();
  results["FullPivHouseholderQR"][id] = t_fpqr.best();
  results["JacobiSVD"][id] = t_jsvd.best();
  results["BDCSVD"][id] = t_bdcsvd.best();
}

int main()
{
  labels.push_back("LLT");
  labels.push_back("LDLT");
  labels.push_back("PartialPivLU");
  labels.push_back("FullPivLU");
  labels.push_back("HouseholderQR");
  labels.push_back("ColPivHouseholderQR");
  labels.push_back("CompleteOrthogonalDecomposition");
  labels.push_back("FullPivHouseholderQR");
  labels.push_back("JacobiSVD");
  labels.push_back("BDCSVD");

  for(int i=0; i<labels.size(); ++i)
    results[labels[i]].fill(-1);

  const int small = 8;
  sizes.push_back(Array2i(small,small));
  sizes.push_back(Array2i(100,100));
  sizes.push_back(Array2i(1000,1000));
  sizes.push_back(Array2i(4000,4000));
  sizes.push_back(Array2i(10000,small));
  sizes.push_back(Array2i(10000,100));
  sizes.push_back(Array2i(10000,1000));
  sizes.push_back(Array2i(10000,4000));

  using namespace std;

  for(int k=0; k<sizes.size(); ++k)
  {
    cout << sizes[k](0) << "x" << sizes[k](1) << "...\n";
    bench<float,Dynamic>(k,sizes[k](0),sizes[k](1));
  }

  cout.width(32);
  cout << "solver/size";
  cout << "  ";
  for(int k=0; k<sizes.size(); ++k)
  {
    std::stringstream ss;
    ss << sizes[k](0) << "x" << sizes[k](1);
    cout.width(10); cout << ss.str(); cout << " ";
  }
  cout << endl;

  for(int i=0; i<labels.size(); ++i)
  {
    cout.width(32); cout << labels[i]; cout << "  ";
    ArrayXf r = (results[labels[i]]*100000.f).floor()/100.f;
    for(int k=0; k<sizes.size(); ++k)
    {
      cout.width(10);
      if(r(k)>=1e6)  cout << "-";
      else           cout << r(k);
      cout << " ";
    }
    cout << endl;
  }

  // HTML output
  cout << "
" << endl;
  cout << "
solver/size	" << endl;
  for(int k=0; k<sizes.size(); ++k)
    cout << "  " << sizes[k](0) << "x" << sizes[k](1) << "	";
  cout << "
" << endl;
  for(int i=0; i<labels.size(); ++i)
  {
    cout << "<tr";
    if(i%2==1) cout << " class=\"alt\"";
    cout << ">" << labels[i] << "	";
    ArrayXf r = (results[labels[i]]*100000.f).floor()/100.f;
    for(int k=0; k<sizes.size(); ++k)
    {
      if(r(k)>=1e6) cout << "-	";
      else
      {
        cout << "" << r(k);
        if(i>0)
          cout << " (x" << numext::round(10.f*results[labels[i]](k)/results["LLT"](k))/10.f << ")";
        if(i<4 && sizes[k](0)!=sizes[k](1))
          cout << " *";
        cout << "	";
      }
    }
    cout << "
" << endl;
  }
  cout << "
" << endl;

//   cout << "LLT                             (ms)  " << (results["LLT"]*1000.).format(fmt) << "\n";
//   cout << "LDLT                             (%)  " << (results["LDLT"]/results["LLT"]).format(fmt) << "\n";
//   cout << "PartialPivLU                     (%)  " << (results["PartialPivLU"]/results["LLT"]).format(fmt) << "\n";
//   cout << "FullPivLU                        (%)  " << (results["FullPivLU"]/results["LLT"]).format(fmt) << "\n";
//   cout << "HouseholderQR                    (%)  " << (results["HouseholderQR"]/results["LLT"]).format(fmt) << "\n";
//   cout << "ColPivHouseholderQR              (%)  " << (results["ColPivHouseholderQR"]/results["LLT"]).format(fmt) << "\n";
//   cout << "CompleteOrthogonalDecomposition  (%)  " << (results["CompleteOrthogonalDecomposition"]/results["LLT"]).format(fmt) << "\n";
//   cout << "FullPivHouseholderQR             (%)  " << (results["FullPivHouseholderQR"]/results["LLT"]).format(fmt) << "\n";
//   cout << "JacobiSVD                        (%)  " << (results["JacobiSVD"]/results["LLT"]).format(fmt) << "\n";
//   cout << "BDCSVD                           (%)  " << (results["BDCSVD"]/results["LLT"]).format(fmt) << "\n";
}


#include "rectangle.hpp"

#include <cmath>
#include <iostream>
#include <stdexcept>

#include <boost/format.hpp>

#include "base-types.hpp"

yakovlev::Rectangle::Rectangle(double width, double height, const point_t & pos, double rotation) :
  vertices_{
    { pos.x + width / 2.0, pos.y + height / 2.0 },
    { pos.x + width / 2.0, pos.y - height / 2.0 },
    { pos.x - width / 2.0, pos.y - height / 2.0 },
    { pos.x - width / 2.0, pos.y + height / 2.0 }
  }
{
  if (width <= 0.0) {
    throw std::invalid_argument{ (boost::format("Invalid rectangle width %1% - can not be negative or zero") % width ).str() };
  }
  if (height <= 0.0) {
    throw std::invalid_argument{ (boost::format("Invalid rectangle height %1% - can not be negative or zero") % height ).str() };
  }
  rotate(rotation);
}

bool yakovlev::Rectangle::operator==(const Rectangle & other) const noexcept
{
  for (const point_t & vert : vertices_) {
    bool hasEqual = false;
    for (const point_t & othervert : other.vertices_) {
      if (vert == othervert) {
        hasEqual = true;
      }
    }
    if (!hasEqual) {
      return false;
    }
  }
  return true;
}

bool yakovlev::Rectangle::operator!=(const Rectangle & other) const noexcept
{
  return !(*this == other);
}

double yakovlev::Rectangle::getWidth() const noexcept
{
  return getDistanceBetweenPoints(vertices_[1], vertices_[2]);
}

double  yakovlev::Rectangle::getHeight() const noexcept
{
  return getDistanceBetweenPoints(vertices_[0], vertices_[1]);
}

yakovlev::point_t yakovlev::Rectangle::getCenter() const noexcept
{
  double sumX = vertices_[0].x;
  double sumY = vertices_[0].y;
  for (size_t i = 1u; i < 4u; ++i) {
    sumX += vertices_[i].x;
    sumY += vertices_[i].y;
  }
  return { sumX / 4.0, sumY / 4.0 };
}

double yakovlev::Rectangle::getArea() const noexcept
{
  return getWidth() * getHeight();
}

yakovlev::rectangle_t yakovlev::Rectangle::getFrameRect() const noexcept
{
  double maxX = vertices_[0].x, maxY = vertices_[0].y;
  double minX = vertices_[0].x, minY = vertices_[0].y;
  for (int i = 1; i < 4; ++i) {
    minX = std::min(vertices_[i].x, minX);
    maxX = std::max(vertices_[i].x, maxX);
    minY = std::min(vertices_[i].y, minY);
    maxY = std::max(vertices_[i].y, maxY);
  }

  double width = maxX - minX, height = maxY - minY;
  return { width, height, { minX + (width / 2.0), minY + (height / 2.0) } };
}

void yakovlev::Rectangle::move(const point_t & pos) noexcept
{
  point_t center = getCenter();
  move(pos.x - center.x, pos.y - center.y);
}

void yakovlev::Rectangle::move(double x, double y) noexcept
{
  for (point_t & vert : vertices_) {
    vert.x += x;
    vert.y += y;
  }
}

void yakovlev::Rectangle::rotate(double rotation) noexcept
{
  double rad = rotation * M_PI / 180.0;
  double rotSin = sin(rad);
  double rotCos = cos(rad);
  point_t pos = getCenter();
  for (point_t & vert : vertices_) {
    point_t local = { vert.x - pos.x, vert.y - pos.y };
    vert = {
        pos.x + local.x * rotCos - local.y * rotSin,
        pos.y + local.x * rotSin + local.y * rotCos
    };
  }
}

void yakovlev::Rectangle::scale(double coef)
{
  if (coef <= 0.0) {
    throw std::invalid_argument{ (boost::format("Invalid rectangle scale coefficient %1% - can not be negative or zero") % coef ).str() };
  }
  point_t pos = getCenter();
  for (point_t & vert : vertices_) {
    vert = {
        pos.x + (vert.x - pos.x) * coef,
        pos.y + (vert.y - pos.y) * coef
    };
  }
}

void yakovlev::Rectangle::print(std::ostream & os) const
{
  os << "Rectangle " << getWidth() << " wide, " << getHeight()
      << " high placed on " << getCenter() << '\n';
}


#include "KDE.hpp"
#include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm/for_each.hpp>
#include <boost/range/irange.hpp>
#include <boost/range/numeric.hpp>
#include <random>
#include <math.h>

using namespace std;
using namespace delphi::utils;
using boost::irange, boost::adaptors::transformed, boost::lambda::_1;

double sample_from_normal(
    std::mt19937 gen,
    double mu = 0.0, /**< The mean of the distribution.*/
    double sd = 1.0  /**< The standard deviation of the distribution.*/
) {
  normal_distribution<> d{mu, sd};
  return d(gen);
}

KDE::KDE(std::vector<double> v) : dataset(v) {

  // Compute the bandwidth using Silverman's rule
  mu = mean(v);
  auto X = v | transformed(_1 - mu);

  // Compute standard deviation of the sample.
  size_t N = v.size();
  double stdev = sqrt(inner_product(X, X, 0.0) / (N - 1));
  bw = pow(4 * pow(stdev, 5) / (3 * N), 1 / 5);
}

KDE::KDE(vector<double> thetas, int n_bins)  : n_bins(n_bins) {
  this->dataset = thetas;
  this->mu = mean(thetas);
  double small_count = 0.00001; // To avoid log(0)
  this->log_prior_hist = vector<double>(n_bins, small_count);
  this->delta_theta = M_PI / n_bins;

  int highest_freq = 0;
  int highest_freq_bin = 0;

//  int bin_lo = n_bins - 1;
//  int bin_hi = 0;

  for (double theta : thetas) {
    theta = theta < 0 ? M_PI + theta : theta;

    int bin = this->theta_to_bin(theta);
//    bin_lo = bin < bin_lo ? bin : bin_lo;
//    bin_hi = bin > bin_hi ? bin : bin_hi;

    this->log_prior_hist[bin] += 1;

    if (highest_freq < this->log_prior_hist[bin]) {
      highest_freq = this->log_prior_hist[bin];
      highest_freq_bin = bin;
    }
  }

//  if (bin_lo != bin_hi && bin_lo != (bin_hi + 1) % n_bins)

  this->most_probable_theta = highest_freq_bin * this->delta_theta +
                              this->delta_theta / 2;
  double n_points = thetas.size() + small_count * n_bins;

  for (double & count : this->log_prior_hist) {
    count /= n_points;
    count = log(count);
  }
}

void KDE::set_num_bins(int n_bins) {
  this->n_bins = n_bins;
  this->delta_theta = M_PI / n_bins;
}

int KDE::theta_to_bin(double theta) {
    return floor(theta / this->delta_theta);
}

vector<double> KDE::resample(int n_samples,
                             std::mt19937& gen,
                             uniform_real_distribution<double>& uni_dist,
                             normal_distribution<double>& norm_dist) {
  vector<double> samples(n_samples);

  for (int i : irange(0, n_samples)) {
    double element = select_random_element(dataset, gen, uni_dist);

    // Transform the sampled values using a Gaussian distribution
    // ~ ( sampled value, bw)
    // We sample from a standard Gaussian and transform that sample
    // to the desired Gaussian distribution by
    // μ + σ * standard Gaussian sample
    samples[i] = element + bw * norm_dist(gen);
  }

  return samples;
}

// This Should not be called!
double KDE::pdf(double x) {
  double p = 0.0;
  size_t N = this->dataset.size();
  for (double elem : this->dataset) {
    double x1 = exp(-sqr(x - elem) / (2 * sqr(bw)));
    x1 /= N * bw * sqrt(2 * M_PI);
    p += x1;
  }
  return p;
}

vector<double> KDE::pdf(vector<double> v) {
  vector<double> values;
  for (double elem : v) {
    values.push_back(pdf(elem));
  }
  return values;
}

double KDE::logpdf(double x) { return log(pdf(x)); }


#include <stdio.h>
#include <math.h>
#include <iostream>
#include <Eigen/Eigen>

Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> loge(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> in)
{
  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> out = in;

  out = out.array() + 1e-7f;

  for(int i=0; i<in.cols(); i++)
    {
      out(0, i) = logf(out(0,i));
    }

  return out;
}

struct NNLayer {
  struct NNLayer *next;
  struct NNLayer *back;

  NNLayer() : next(NULL), back(NULL)
    {
    };

  ~NNLayer() {};

  virtual Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> forward(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> m) = 0;

  virtual void back_propagation() = 0;

  void setNext(struct NNLayer *n)
    {
      next = n;
      n->back = this;
    };

  struct NNLayer * getNext()
    {
      return next;
    };

};

class Activation : public NNLayer {
  public:
    virtual Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> forward(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> m)
      {
      };
};

struct AffineLayer : public NNLayer {
  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> w;
  Eigen::Matrix<float, 1, Eigen::Dynamic> bias;
  Eigen::Matrix<float, 1, Eigen::Dynamic> output;
  bool needActivation;

  AffineLayer() : NNLayer(), needActivation(true)
    {
    }

  AffineLayer(int raws, int cols) : NNLayer(), needActivation(true)
    {
      resize(raws, cols);
    }

  ~AffineLayer() {};

  void resize(int raws, int cols)
    {
      w.resize(raws, cols);
      bias.resize(1, cols);
      output.resize(1, cols);

      w      = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>::Random(raws, cols);
      bias   = Eigen::Matrix<float, 1, Eigen::Dynamic>::Random(1, cols);
      output = Eigen::Matrix<float, 1, Eigen::Dynamic>::Random(1, cols);
    }

  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> forward(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> m)
    {
      output = (m * w) - bias;
      if (needActivation) activate();

      if (next != NULL)
        {
          return next->forward(output);
        }
      else
        {
          return output;
        }
    }

  void back_propagation()
    {
    };

  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> activate()
    {
      for(int i=0; i<output.cols(); i++)
        {
          output(0, i) = 1.0/(1.0 + exp(output(0, i)));
        }
      return output;
    }

  void setActivation(bool isneed)
    {
      needActivation = isneed;
    }
};

class NeuralNetwork {
  NNLayer *top_layer;
  NNLayer *last_layer;
  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> output;

  public:
    NeuralNetwork()
      {
        AffineLayer * tmplayers = new AffineLayer[3];
        tmplayers[0].resize(2,3);
        tmplayers[1].resize(3,5);
        tmplayers[2].resize(5,2);
        // tmplayers[2].setActivation(false);

        tmplayers[0].setNext(&tmplayers[1]);
        tmplayers[1].setNext(&tmplayers[2]);

        top_layer  = &tmplayers[0];
        last_layer = &tmplayers[2];
      };
    ~NeuralNetwork()
      {
        delete [] top_layer;
      };

    Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> forward(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> input)
      {
        output = top_layer->forward(input);
        return output;
      };

    void back_propagation()
      {
      };

    float cross_entropy(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> in)
      {
        Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> tmp;
        float out = 0.f;
        tmp = in.array() * loge(output).array();
        for(int i=0; i<tmp.cols(); i++)
          {
            out -= tmp(0, i);
          }
        return out;
      };
};

int main(void)
{
  NeuralNetwork nn;

  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> out;
  Eigen::Matrix<float, 1, 2> input;
  Eigen::Matrix<float, 1, 2> expect;
  input << 3, 3;

  expect << 2, 5;

  std::cout << "input = " << input << std::endl;
  std::cout << "expect = " << expect << std::endl;
  std::cout << "input * expect = " << input.array() * expect.array() << std::endl;

  std::cout << "input = " << input << std::endl;
  input = input.array() + 1;
  std::cout << "input = " << input << std::endl;

  out = nn.forward( input );

  std::cout << "out = " << out << std::endl;

  std::cout << "CrossEnt = " << nn.cross_entropy(expect) << std::endl;

  return 0;
}


/* $Id: step-3.cc 24232 2011-09-02 09:47:37Z kronbichler $ */
/* Author: Wolfgang Bangerth, 1999, Guido Kanschat, 2011 */

/*    $Id: step-3.cc 24232 2011-09-02 09:47:37Z kronbichler $       */
/*                                                                */
/*    Copyright (C) 1999, 2000, 2001, 2002, 2003, 2004, 2006, 2007, 2008, 2010, 2011 by the deal.II authors */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the  text  and     */
/*    further information on this license.                        */

/* Modified for Exercise 2.5 of the finite element lecture
   in Hamburg in Summer 2014 by W. Wollner
*/

#include <deal.II/grid/tria.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.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/compressed_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/numerics/data_out.h>
#include <fstream>
#include <iostream>

using namespace dealii;

class Solution : public Function<2>
{
public:
  Solution () : Function<2>() {}

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

  Tensor<1,2> gradient (const Point<2>   &p,
			const unsigned int  component = 0) const;
};

double Solution::value(const Point<2>   &p, const unsigned int) const
{
  return sin(M_PI * p(0))*sin(2.*M_PI * p(1));
}

Tensor<1,2> Solution::gradient (const Point<2>   &p,
				const unsigned int) const
{
  //EXERCISE: This is used to evaluate the gradient of the
  //known reference solution and needs to be implemented.
  //If you don't know how to do this have a look into the
  //deal.II steps 1-3 and the function evaluation
  //the value of this function above.
  Tensor<1,2> return_value;
  return_value[0] = M_PI * cos(M_PI * p(0)) * sin(2.*M_PI * p(1));
  return_value[1] = 2 * M_PI * sin(M_PI * p(0)) * cos(2.*M_PI * p(1));
  return return_value;
}

class Problem
{
  public:
    Problem (unsigned int deg);
    void run ();
    void summarize_results () const;

  private:
    void refine_grid();
    void setup_system ();
    void assemble_system ();
    void solve ();
    void output_results ();
    virtual void make_grid (unsigned int ref);

    Triangulation<2>     triangulation;

    FE_Q<2>              fe;
    DoFHandler<2>        dof_handler;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

    Vector<double>       solution;
    Vector<double>       system_rhs;
    std::vector<double> dofs;
    std::vector<double> l2_value;
    std::vector<double> h1_value;
    unsigned int n_iter;
    unsigned int iter;
};

Problem::Problem (unsigned int deg)
		:
                fe (deg),
		dof_handler (triangulation)
{
  n_iter = 5;
  dofs.resize(n_iter);
  l2_value.resize(n_iter);
  h1_value.resize(n_iter);
  iter = 0;
}

void Problem::make_grid (unsigned int ref)
{
  GridGenerator::hyper_cube (triangulation);
  triangulation.refine_global (ref);
  std::cout << "Number of active cells: "
	    << triangulation.n_active_cells()
	    << std::endl;
  std::cout << "Total number of cells: "
	    << triangulation.n_cells()
	    << std::endl;
}
void Problem::refine_grid ()
{
  triangulation.refine_global (1);
  std::cout << std::endl;
  std::cout << "Refining the triangulation ..."<<std::endl;
  std::cout << "Number of active cells: "
	    << triangulation.n_active_cells()
	    << std::endl;
  std::cout << "Total number of cells: "
	    << triangulation.n_cells()
	    << std::endl;
}
void Problem::setup_system ()
{
  dof_handler.distribute_dofs (fe);
  std::cout << "Number of degrees of freedom: "
	    << dof_handler.n_dofs()
	    << std::endl;
  dofs[iter] = dof_handler.n_dofs();

  CompressedSparsityPattern c_sparsity(dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern (dof_handler, c_sparsity);
  sparsity_pattern.copy_from(c_sparsity);

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

void Problem::assemble_system ()
{
  QGauss<2>  quadrature_formula(2);
  FEValues<2> fe_values (fe, quadrature_formula,
			 update_quadrature_points | update_values | update_gradients | update_JxW_values);
  const unsigned int   dofs_per_cell = fe.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<unsigned int> local_dof_indices (dofs_per_cell);

  DoFHandler<2>::active_cell_iterator
    cell = dof_handler.begin_active(),
    endc = dof_handler.end();
  for (; cell!=endc; ++cell)
    {
      fe_values.reinit (cell);
      cell_matrix = 0;
      cell_rhs = 0;
      for (unsigned int i=0; i<dofs_per_cell; ++i)
	for (unsigned int j=0; j<dofs_per_cell; ++j)
	  for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
	    cell_matrix(i,j) += (fe_values.shape_grad (i, q_point) *
				 fe_values.shape_grad (j, q_point) *
				 fe_values.JxW (q_point));
      for (unsigned int i=0; i<dofs_per_cell; ++i)
	for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
	{

 	  //EXERCISE
 	  /* This should implement the required right hand side
 	   *  so you need to implement the integrand of
 	   *  \int_\Omega f \phi \, dx
 	   *  Since you use a quadrature formula, you only need to write
 	   *  The value in the quadrature point (q_point)
 	   *  which you can access using
 	   *  fe_values.quadrature_point(q_point)
 	   *  Currently the righthand side f = 1 is implemented.
 	   *  The value of the test function is accessible using
 	   *  fe_values.shape_value (i, q_point)
 	   *
 	   *  Note: do not remove the term fe_values.JxW (q_point)
 	   *  it contains the quadrature weights!
 	   */
	  Point<2> c_point = fe_values.quadrature_point(q_point);
	  cell_rhs(i) += (fe_values.shape_value (i, q_point) *
			  5 * M_PI * M_PI * sin(M_PI * c_point(0)) * sin(2 * M_PI * c_point(1)) *
			  fe_values.JxW (q_point));
	}
      cell->get_dof_indices (local_dof_indices);

      for (unsigned int i=0; i<dofs_per_cell; ++i)
	for (unsigned int j=0; j<dofs_per_cell; ++j)
	  system_matrix.add (local_dof_indices[i],
			     local_dof_indices[j],
			     cell_matrix(i,j));

      for (unsigned int i=0; i<dofs_per_cell; ++i)
	system_rhs(local_dof_indices[i]) += cell_rhs(i);
    }

  std::map<unsigned int,double> boundary_values;
  VectorTools::interpolate_boundary_values (dof_handler,
					    0,
					    ZeroFunction<2>(),
					    boundary_values);
  MatrixTools::apply_boundary_values (boundary_values,
				      system_matrix,
				      solution,
				      system_rhs);
}

void Problem::solve ()
{
  SolverControl           solver_control (10000, 1e-12);
  SolverCG<>              solver (solver_control);

  solver.solve (system_matrix, solution, system_rhs,
		PreconditionIdentity());
}

void Problem::output_results ()
{
  DataOut<2> data_out;
  data_out.attach_dof_handler (dof_handler);
  data_out.add_data_vector (solution, "solution");
  data_out.build_patches ();
  std::ofstream output ("solution.gpl");
  data_out.write_gnuplot (output);

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

  VectorTools::integrate_difference (dof_handler,
				     solution,
				     Solution(),
				     difference_per_cell,
				     QGauss<2>(3),
				     VectorTools::L2_norm);
  l2_value[iter] = difference_per_cell.l2_norm();

  std::cout << "L2 Error "
	    << l2_value[iter]
  << std::endl;
  //EXERCISE: Here you should take care to evaluate the
  // H1 seminorm.
  VectorTools::integrate_difference (dof_handler,
				     solution,
				     Solution(),
				     difference_per_cell,
				     QGauss<2>(3),
				     VectorTools::H1_seminorm);
  h1_value[iter] = difference_per_cell.l2_norm();
  std::cout << "H1 Error: "
	    << h1_value[iter]
	    << std::endl;

}

void Problem::summarize_results () const
{
  std::cout<<"DOFS\tL2 Norm\t\tEOC\tH1 Norm\t\tEOC"<<std::endl;
  std::cout<<"--------------------------------------------------"<<std::endl;
  for(unsigned int i = 0; i < n_iter; i++)
  {
    double order_p = 0.;
    double order_m = 0.;
    if(i > 1)
    {
      //EXERCISE:
      /* The following lines need to calculate the estimated order of
       * convergence for the L^2- and H^1 Norm.
       * To do so we have stored the values of the L^2 error on the different
       * triangulations h_1, h_2, h_3 in the array named " l2_value "
       * and those for the H1 seminorm in h1_value.
       */
      order_p = log(l2_value[i-1] / l2_value[i]) / log(2);
      order_m = log(h1_value[i-1] / h1_value[i]) / log(2);
      std::cout<<dofs[i]<<"\t"<<l2_value[i]<<"\t"<<order_p<<"\t"<<h1_value[i]<<"\t"<<order_m<<std::endl;
    }
    else
    {
      std::cout<<dofs[i]<<"\t"<<l2_value[i]<<"\t---\t"<<h1_value[i]<<"\t---"<<std::endl;
    }

  }
}

void Problem::run ()
{
  make_grid (3);
  for(;iter < n_iter; iter++)
  {
    setup_system();
    assemble_system ();
    solve ();
    output_results ();
    refine_grid();
  }
}

int main ()
{
  Problem problem(1);
  problem.run ();
  Problem problem_2(2);
  problem_2.run ();

  std::cout<<"With Bilinear elements:"<<std::endl;
  problem.summarize_results();

  std::cout<<std::endl;
  std::cout<<"With Biquadratic elements::"<<std::endl;
  problem_2.summarize_results();

  return 0;
}


#include "utils.h"

#include <NTL/GF2EX.h>
#include <NTL/GF2X.h>
#include <stdexcept>

using namespace NTL;

namespace utils {

static GF2X modulus;

void init_ntl_extension_field(NTL_INSTANCE instance) {
  switch (instance) {
  case GF2_128: {
    // modulus = x^128 + x^7 + x^2 + x^1 + 1
    clear(modulus);
    SetCoeff(modulus, 128);
    SetCoeff(modulus, 7);
    SetCoeff(modulus, 2);
    SetCoeff(modulus, 1);
    SetCoeff(modulus, 0);
    GF2E::init(modulus);
  } break;
  case GF2_192: {
    // modulus = x^192 + x^7 + x^2 + x^1 + 1
    clear(modulus);
    SetCoeff(modulus, 192);
    SetCoeff(modulus, 7);
    SetCoeff(modulus, 2);
    SetCoeff(modulus, 1);
    SetCoeff(modulus, 0);
    GF2E::init(modulus);
  } break;
  case GF2_256: {
    // modulus = x^256 + x^10 + x^5 + x^2 + 1
    clear(modulus);
    SetCoeff(modulus, 256);
    SetCoeff(modulus, 10);
    SetCoeff(modulus, 5);
    SetCoeff(modulus, 2);
    SetCoeff(modulus, 0);
    GF2E::init(modulus);
  } break;
  default:
    throw std::runtime_error("instance not implemented.");
  }
}

GF2E GF2E_from_bytes(const std::vector<uint8_t> &value) {
  // assumes value is already smaller than current modulus
  GF2X inner = GF2XFromBytes(value.data(), value.size());
  return conv<GF2E>(inner);
}

vec_GF2E get_first_n_field_elements(size_t n) {
  vec_GF2E result;
  result.SetLength(n);
  GF2X gen;
  SetX(gen);
  for (size_t i = 0; i < n; i++) {
    result[i] = conv<GF2E>(gen);
    gen = MulByX(gen);
  }
  return result;
}
std::vector<GF2EX> precompute_lagrange_polynomials(const vec_GF2E &x_values) {
  size_t m = x_values.length();
  std::vector<GF2EX> precomputed_lagrange_polynomials;
  precomputed_lagrange_polynomials.reserve(m);

  GF2EX full_poly = BuildFromRoots(x_values);
  GF2EX lagrange_poly;
  GF2EX missing_term;
  SetX(missing_term);
  for (size_t k = 0; k < m; k++) {
    SetCoeff(missing_term, 0, -x_values[k]);
    lagrange_poly = full_poly / missing_term;
    lagrange_poly = lagrange_poly / eval(lagrange_poly, x_values[k]);
    precomputed_lagrange_polynomials.push_back(lagrange_poly);
  }

  return precomputed_lagrange_polynomials;
}

GF2EX interpolate_with_precomputation(
    const std::vector<GF2EX> &precomputed_lagrange_polynomials,
    const vec_GF2E &y_values) {
  if (precomputed_lagrange_polynomials.size() != (size_t)y_values.length())
    throw std::runtime_error("invalid sizes for interpolation");

  GF2EX res;
  size_t m = y_values.length();
  for (size_t k = 0; k < m; k++) {
    res += precomputed_lagrange_polynomials[k] * y_values[k];
  }
  return res;
}
} // namespace utils


#include "testpendulum.h"
#include <Eigen>
#include <Core>
namespace csv::internals {
using std::to_string;
}
#include "csv.hpp"

using namespace Eigen;

MatrixXd matFromCsv(const std::string& path)
{
    csv::CSVFormat format;
    format.no_header();
    csv::CSVReader reader(path, format);
    MatrixXd mat;

    size_t maxRowSize = 0;
    size_t rowNum = 0;
    bool first = true;
    for(auto& row : reader)
    {
        size_t col = 0;
        if(first)
        {
            maxRowSize = row.size();
            mat.resize(1000, maxRowSize);
            first = false;
        }
        for(auto& field : row)
        {
            if(col > maxRowSize)
            {
                maxRowSize = col;
                mat.conservativeResize(1000, maxRowSize);
            }
            mat(rowNum, col) = field.get<double>();
            col++;
        }
        if(col<maxRowSize)
            for(size_t i=1; i<=col-maxRowSize; i++)
            {
                mat(rowNum, col+i) = 0;
            }
        rowNum++;
        if (rowNum % 1000 == 0)
        {
            mat.conservativeResize(rowNum+1000, maxRowSize);
        }
    }
    mat.conservativeResize(rowNum, maxRowSize);
    return mat;
}

double TestPendulum::d2x(double x1, double y1, double x0, double y0)
{
    Array4d arr = {x1, y1, x0, y0};
    return (arr.transpose()*A1.row(0).array()).sum();
}

double TestPendulum::d2y(double x1, double y1, double x0, double y0)
{
    Array4d arr = {x1, y1, x0, y0};
    return (arr.transpose()*A1.row(1).array()).sum();
}

void TestPendulum::reset()
{
    stop = 500;
    x0 = 60;
    y0 = 0;
    x1 = 0;
    y1 = 0;
    x2 = d2x(x1, y1, x0, y0);
    y2 = d2y(x1, y1, x0, y0);
    prevLinePoint = {x0, 0, y0};
    graph.clearLines();
}

TestPendulum::TestPendulum()
{
    ball = MeshRenderer::createCubeSphere(10);
    graph.setPointSize(8);

    // Import data from CSV files
    auto data1 = matFromCsv("../assets/misc/data1.csv");
    auto data2 = matFromCsv("../assets/misc/data2.csv");

    // Separate matrices
    auto data1X = data1.block(0, 0, data1.rows(), 2).transpose();
    auto data1Z = data1.block(0, 2, data1.rows(), 4).transpose();

    auto data2X = data2.block(0, 0, data2.rows(), 2).transpose();
    auto data2Z = data2.block(0, 2, data2.rows(), 4).transpose();

    // Make [Z;Z^2] matrix
    auto Zsq = pow(data2Z.array(), 2);
    MatrixXd ZAndZsq(Zsq.rows()*2, Zsq.cols());
    ZAndZsq.block(0,0,Zsq.rows(), Zsq.cols()) = data2Z;
    ZAndZsq.block(Zsq.rows(),0,Zsq.rows(), Zsq.cols()) = Zsq;

    // Calculate coefficiants for linear and sqr equations
    A1 = (data2X*data2Z.transpose())*(data2Z*data2Z.transpose()).inverse();
    A2 = (data2X*ZAndZsq.transpose())*(ZAndZsq*ZAndZsq.transpose()).inverse();

    // Calculate MSE1
    vec3 prevPoint = {0,0,0};
    bool first = true;
    double MSEx1 = 0;
    double MSEy1 = 0;
    for(int i=0; i<data1X.cols(); i++)
    {
        auto ddxTrue = data1X(0, i);
        auto ddxComp = (data1Z.col(i).transpose().array() * A1.row(0).array()).sum();
        MSEx1 += (ddxTrue-ddxComp)*(ddxTrue-ddxComp);

        auto ddyTrue = data1X(1, i);
        auto ddyComp = (data1Z.col(i).transpose().array() * A1.row(1).array()).sum();
        MSEy1 += (ddyTrue-ddyComp)*(ddyTrue-ddyComp);

        // Add lines from data1 to graph
        vec3 point = {data1Z(2, i), 0, data1Z(3, i)};
        if(!first)
        {
            if(glm::length(prevPoint-point)>1) // only draw line if the length > 1
            {
                graph2.addLine(prevPoint, point, {0, 1, 0, 1});
                prevPoint = point;
            }
        }
        else
        {
            prevPoint = point;
            first = false;
        }
    }
    MSE1 = (MSEx1+MSEy1)/(data1X.cols()*2);

    // Calculate MSE2
    double MSEx2 = 0;
    double MSEy2 = 0;
    auto Zsq1 = pow(data1Z.array(), 2);
    MatrixXd ZAndZsq1(Zsq1.rows()*2, Zsq1.cols());
    ZAndZsq1.block(0,0,Zsq1.rows(), Zsq1.cols()) = data1Z;
    ZAndZsq1.block(Zsq1.rows(),0,Zsq1.rows(), Zsq1.cols()) = Zsq1;

    for(int i=0; i<data1X.cols(); i++)
    {

        auto ddxTrue = data1X(0, i);
        auto ddxComp = (ZAndZsq1.col(i).transpose().array() * A2.row(0).array()).sum();
        MSEx2 += (ddxTrue-ddxComp)*(ddxTrue-ddxComp);

        auto ddyTrue = data1X(1, i);
        auto ddyComp = (ZAndZsq1.col(i).transpose().array() * A2.row(1).array()).sum();
        MSEy2 += (ddyTrue-ddyComp)*(ddyTrue-ddyComp);
//        std::cout<<"MSEx2 += ("<<ddxTrue<<" - "<<ddxComp<<") ^2\n";
//        std::cout<<"MSEy2 += ("<<ddyTrue<<" - "<<ddyComp<<") ^2\n";
    }
    MSE2 = (MSEx2+MSEy2)/(data1X.cols()*2);
}

void TestPendulum::onStart()
{
    GraphicsContext::setClearColor({0.184f, 0.200f, 0.329f, 1.f});
    auto camera = GraphicsContext::getCamera();
    camera->setFov(50.f);
    camera->setPosition({0 ,50.f,50});
    camera->setFocusPoint({0,0,0});
    reset();
    std::cout<<"A1:\n"<<A1<<"\n\n";
    std::cout<<"A2:\n"<<A2<<"\n\n\n";
    std::cout<<"MSE1: "<<MSE1<<"\n";
    std::cout<<"MSE2: "<<MSE2<<"\n";
}

void TestPendulum::onUpdate(float dt)
{
    // Controls
    if(App::getKeyOnce(GLFW_KEY_KP_SUBTRACT))
        dtMulti -= 1;
    if(App::getKeyOnce(GLFW_KEY_KP_ADD))
        dtMulti += 1;
    if(App::getKeyOnce(GLFW_KEY_SPACE))
        reset();

    // Clear ball and its line
    graph.clearPoints();
    graph.removeLastLine();

    // Simulate pendulum
    double timeToSim = dt*dtMulti;
    if(stop>0)
    {
        uint simSteps = (uint)(timeToSim/h);
        for(uint i=0; i<simSteps; i++)
        {
            x0 = x0 + h*x1;
            x1 = x1 + h*x2;

            y0 = y0 + h*y1;
            y1 = y1 + h*y2;

            x2 = d2x(x1, y1, x0, y0);
            y2 = d2y(x1, y1, x0, y0);
        }
        stop-=timeToSim;
    }
    vec3 point = {x0, 0, y0};
    if(glm::length(prevLinePoint-point)>1)
    {
        graph.addLine(prevLinePoint, point, {1,0,0,1});
        prevLinePoint = point;
    }

    // Draw
    float lh = 90;
    vec3 lv = point - vec3{0, 100, 0};
    point.y = 100-sqrt(lh*lh - lv.x*lv.x - lv.z*lv.z);
    graph.addPoints(&point, 1, {1,0,0,1});
    graph.addLine({0, 100, 0}, point, {1,1,1,1});
    graph.draw ({-22,0,0},dt);
    graph2.draw({ 2,0,0},dt);
}


#include <Eigen/Dense>
#include <Eigen/LU>

#include "apriltag_mit/AprilTags/GrayModel.h"

namespace AprilTags {

void GrayModel::AddBlackObs(float x, float y, float v) {
  black_model_.AddObservation(x, y, v);
}

void GrayModel::AddWhiteObs(float x, float y, float v) {
  white_model_.AddObservation(x, y, v);
}

void GrayModel::Fit() {
  black_model_.Fit();
  white_model_.Fit();
}

float GrayModel::CalcThreshold(float x, float y) const {
  return (black_model_.Predict(x, y) + white_model_.Predict(x, y)) / 2;
}

IntensityModel::IntensityModel()
    : A_(), c_(), b_(), num_obs_(0), dirty_(false) {
  A_.setZero();
  c_.setZero();
  b_.setZero();
}

void IntensityModel::AddObservation(float x, float y, float v) {
  float xy = x * y;

  // update only upper-right elements. A'A is symmetric,
  // we'll fill the other elements in later.
  A_(0, 0) += x * x;
  A_(0, 1) += x * y;
  A_(0, 2) += x * xy;
  A_(0, 3) += x;
  A_(1, 1) += y * y;
  A_(1, 2) += y * xy;
  A_(1, 3) += y;
  A_(2, 2) += xy * xy;
  A_(2, 3) += xy;
  A_(3, 3) += 1;

  b_[0] += x * v;
  b_[1] += y * v;
  b_[2] += xy * v;
  b_[3] += v;

  num_obs_++;
  dirty_ = true;
}

float IntensityModel::Predict(float x, float y) const {
  return c_[0] * x + c_[1] * y + c_[2] * x * y + c_[3];
}

void IntensityModel::Fit() {
  // we really only need 4 linearly independent observations to fit our answer,
  // but we'll be very sensitive to noise if we don't have an over-determined
  // system. Thus, require at least 6 observations (or we'll use a constant
  // model below).

  if (num_obs_ >= 6) {
    // make symmetric
    for (int i = 0; i < 4; ++i) {
      for (int j = i + 1; j < 4; ++j) {
        A_(j, i) = A_(i, j);
      }
    }

    bool invertible;
    double det_unused;
    Eigen::Matrix4d Ainv;
    A_.computeInverseAndDetWithCheck(Ainv, det_unused, invertible);
    if (invertible) {
      c_ = Ainv * b_;
      return;
    }
  }

  // If we get here, either nobs < 6 or the matrix inverse generated
  // an underflow, so use a constant model.
  c_.setZero(); // need the cast to avoid operator= ambiguity wrt. const-ness
  c_[3] = b_[3] / num_obs_;
}

bool IsOnOuterBorder(int x, int y, int l, bool black_corner) {
  if (black_corner) {
    // Black corners will be excluded
    const bool on_corner = (x == -1 && y == -1) || (x == -1 && y == l) ||
                           (x == l && y == -1) || (x == l && y == l);
    return (y == -1 || y == l || x == -1 || x == l) && !on_corner;
  } else {
    return (y == -1 || y == l || x == -1 || x == l);
  }
}

bool IsOnInnerBorder(int x, int y, int l) {
  return (y == 0 || y == (l - 1) || x == 0 || x == (l - 1));
}

bool IsInsideInnerBorder(int x, int y, int l) {
  return (y >= 1 && y < (l - 1) && x >= 1 && x < (l - 1));
}

} // namespace AprilTags


#include <catch2/catch.hpp>
#include <Euclid/Geometry/TriMeshGeometry.h>

#include <string>
#include <vector>

#include <CGAL/Simple_cartesian.h>
#include <CGAL/Surface_mesh.h>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseCholesky>
#include <Euclid/IO/OffIO.h>
#include <Euclid/IO/PlyIO.h>
#include <Euclid/MeshUtil/CGALMesh.h>
#include <Euclid/MeshUtil/EigenMesh.h>
#include <Euclid/Util/Color.h>
#include <igl/invert_diag.h>

#include <config.h>

using Kernel = CGAL::Simple_cartesian<float>;
using Point_3 = typename Kernel::Point_3;
using Vector_3 = typename Kernel::Vector_3;
using Mesh = CGAL::Surface_mesh<Point_3>;

TEST_CASE("Geometry, TriMeshGeometry", "[geometry][trimeshgeometry]")
{
    std::string fcube(DATA_DIR);
    fcube.append("cube_ascii.ply");
    std::vector<float> cpositions;
    std::vector<int> cindices;
    Euclid::read_ply<3>(
        fcube, cpositions, nullptr, nullptr, &cindices, nullptr);
    Mesh cube;
    Euclid::make_mesh<3>(cube, cpositions, cindices);

    std::string fbumpy(DATA_DIR);
    fbumpy.append("bumpy.off");
    std::vector<float> bpositions;
    std::vector<int> bindices;
    Euclid::read_off<3>(fbumpy, bpositions, nullptr, &bindices, nullptr);
    Mesh bumpy;
    Euclid::make_mesh<3>(bumpy, bpositions, bindices);
    Eigen::MatrixXf bumpy_v;
    Eigen::MatrixXi bumpy_f;
    Euclid::make_mesh<3>(bumpy_v, bumpy_f, bpositions, bindices);

    SECTION("vertex normal")
    {
        auto fnormals = Euclid::face_normals(bumpy);

        SECTION("uniform")
        {
            auto w = Euclid::VertexNormal::uniform;
            std::vector<float> vnormals1;
            for (auto v : vertices(bumpy)) {
                auto vn1 = Euclid::vertex_normal(v, bumpy, w);
                auto vn2 = Euclid::vertex_normal(v, bumpy, fnormals, w);
                REQUIRE(vn1 == vn2);
                vnormals1.push_back(vn1.x());
                vnormals1.push_back(vn1.y());
                vnormals1.push_back(vn1.z());
            }
            auto vnormals2 = Euclid::vertex_normals(bumpy, fnormals, w);
            REQUIRE(vnormals1.size() == vnormals2.size() * 3);
            REQUIRE(vnormals1[0] == vnormals2[0].x());
            REQUIRE(vnormals1[1] == vnormals2[0].y());
            REQUIRE(vnormals1[2] == vnormals2[0].z());

            for (auto& n : vnormals1) {
                n = (n + 1.0f) * 127.5f;
            }
            std::string fout(TMP_DIR);
            fout.append("bumpy_vn_uniform.ply");
            Euclid::write_ply<3>(
                fout, bpositions, nullptr, nullptr, &bindices, &vnormals1);
        }

        SECTION("face_area")
        {
            auto w = Euclid::VertexNormal::face_area;
            std::vector<float> vnormals1;
            for (auto v : vertices(bumpy)) {
                auto vn1 = Euclid::vertex_normal(v, bumpy, w);
                auto vn2 = Euclid::vertex_normal(v, bumpy, fnormals, w);
                REQUIRE(vn1 == vn2);
                vnormals1.push_back(vn1.x());
                vnormals1.push_back(vn1.y());
                vnormals1.push_back(vn1.z());
            }
            auto vnormals2 = Euclid::vertex_normals(bumpy, fnormals, w);
            REQUIRE(vnormals1.size() == vnormals2.size() * 3);
            REQUIRE(vnormals1[0] == vnormals2[0].x());
            REQUIRE(vnormals1[1] == vnormals2[0].y());
            REQUIRE(vnormals1[2] == vnormals2[0].z());

            for (auto& n : vnormals1) {
                n = (n + 1.0f) * 127.5f;
            }
            std::string fout(TMP_DIR);
            fout.append("bumpy_vn_area.ply");
            Euclid::write_ply<3>(
                fout, bpositions, nullptr, nullptr, &bindices, &vnormals1);
        }

        SECTION("incident angle")
        {
            auto w = Euclid::VertexNormal::incident_angle;
            std::vector<float> vnormals1;
            for (auto v : vertices(bumpy)) {
                auto vn1 = Euclid::vertex_normal(v, bumpy, w);
                auto vn2 = Euclid::vertex_normal(v, bumpy, fnormals, w);
                REQUIRE(vn1 == vn2);
                vnormals1.push_back(vn1.x());
                vnormals1.push_back(vn1.y());
                vnormals1.push_back(vn1.z());
            }
            auto vnormals2 = Euclid::vertex_normals(bumpy, fnormals, w);
            REQUIRE(vnormals1.size() == vnormals2.size() * 3);
            REQUIRE(vnormals1[0] == vnormals2[0].x());
            REQUIRE(vnormals1[1] == vnormals2[0].y());
            REQUIRE(vnormals1[2] == vnormals2[0].z());

            for (auto& n : vnormals1) {
                n = (n + 1.0f) * 127.5f;
            }
            std::string fout(TMP_DIR);
            fout.append("bumpy_vn_angle.ply");
            Euclid::write_ply<3>(
                fout, bpositions, nullptr, nullptr, &bindices, &vnormals1);
        }
    }

    SECTION("veretx area")
    {
        SECTION("barycentric")
        {
            auto w = Euclid::VertexArea::barycentric;
            std::vector<float> vareas1;
            for (auto v : vertices(bumpy)) {
                auto va = Euclid::vertex_area(v, bumpy, w);
                vareas1.push_back(va);
                vareas1.push_back(0.0f);
                vareas1.push_back(0.0f);
            }
            auto vareas2 = Euclid::vertex_areas(bumpy, w);
            REQUIRE(vareas1.size() == vareas2.size() * 3);
            REQUIRE(vareas1[0] == vareas2[0]);

            auto amax = *std::max_element(vareas1.begin(), vareas1.end());
            amax = 1.0f / amax;
            for (auto& a : vareas1) {
                a *= amax * 255.0f;
            }
            std::string fout(TMP_DIR);
            fout.append("bumpy_va_barycentric.ply");
            Euclid::write_ply<3>(
                fout, bpositions, nullptr, nullptr, &bindices, &vareas1);
        }

        SECTION("voronoi")
        {
            auto w = Euclid::VertexArea::voronoi;
            std::vector<float> vareas1;
            for (auto v : vertices(bumpy)) {
                auto va = Euclid::vertex_area(v, bumpy, w);
                vareas1.push_back(va);
                vareas1.push_back(0.0f);
                vareas1.push_back(0.0f);
            }
            auto vareas2 = Euclid::vertex_areas(bumpy, w);
            REQUIRE(vareas1.size() == vareas2.size() * 3);
            REQUIRE(vareas1[0] == vareas2[0]);

            auto amax = *std::max_element(vareas1.begin(), vareas1.end());
            amax = 1.0f / amax;
            for (auto& a : vareas1) {
                a *= amax * 255.0f;
            }
            std::string fout(TMP_DIR);
            fout.append("bumpy_va_voronoi.ply");
            Euclid::write_ply<3>(
                fout, bpositions, nullptr, nullptr, &bindices, &vareas1);
        }

        SECTION("mixed voronoi")
        {
            auto w = Euclid::VertexArea::mixed_voronoi;
            std::vector<float> vareas1;
            for (auto v : vertices(bumpy)) {
                auto va = Euclid::vertex_area(v, bumpy, w);
                vareas1.push_back(va);
                vareas1.push_back(0.0f);
                vareas1.push_back(0.0f);
            }
            auto vareas2 = Euclid::vertex_areas(bumpy, w);
            REQUIRE(vareas1.size() == vareas2.size() * 3);
            REQUIRE(vareas1[0] == vareas2[0]);

            auto amax = *std::max_element(vareas1.begin(), vareas1.end());
            amax = 1.0f / amax;
            for (auto& a : vareas1) {
                a *= amax * 255.0f;
            }
            std::string fout(TMP_DIR);
            fout.append("bumpy_va_mixed.ply");
            Euclid::write_ply<3>(
                fout, bpositions, nullptr, nullptr, &bindices, &vareas1);
        }
    }

    SECTION("edge length and squared length")
    {
        auto [ebeg, eend] = edges(cube);
        auto e = *(++ebeg);
        auto he = halfedge(e, cube);
        auto elens = Euclid::edge_lengths(cube);
        auto sq_elens = Euclid::squared_edge_lengths(cube);

        REQUIRE(Euclid::edge_length(e, cube) == Approx(2.0f));
        REQUIRE(Euclid::edge_length(he, cube) == Approx(2.0f));
        REQUIRE(elens[1] == Approx(2.0f));
        REQUIRE(elens.size() == num_edges(cube));

        REQUIRE(Euclid::squared_edge_length(e, cube) == Approx(4.0f));
        REQUIRE(Euclid::squared_edge_length(he, cube) == Approx(4.0f));
        REQUIRE(sq_elens[1] == Approx(4.0f));
        REQUIRE(sq_elens.size() == num_edges(cube));
    }

    SECTION("face normal")
    {
        auto [fbeg, fend] = faces(cube);
        auto f = *fbeg;
        REQUIRE(Euclid::face_normal(f, cube) == Vector_3(0.0f, 0.0f, -1.0f));

        auto fnormals = Euclid::face_normals(cube);
        REQUIRE(fnormals.size() == num_faces(cube));
        REQUIRE(fnormals[0] == Vector_3(0.0f, 0.0f, -1.0f));
    }

    SECTION("face area")
    {
        auto [fbeg, fend] = faces(cube);
        auto f = *fbeg;
        auto fareas = Euclid::face_areas(cube);

        REQUIRE(Euclid::face_area(f, cube) == 2.0f);
        REQUIRE(fareas.size() == num_faces(cube));
        REQUIRE(fareas[0] == 2.0f);
    }

    SECTION("face barycenter")
    {
        auto [fbeg, fend] = faces(cube);
        auto f = *fbeg;
        auto c = Euclid::barycenter(f, cube);
        auto centroids = Euclid::barycenters(cube);

        REQUIRE(c.x() == Approx(-1.0 / 3.0));
        REQUIRE(c.y() == Approx(1.0 / 3.0));
        REQUIRE(c.z() == -1.0);
        REQUIRE(centroids[0] == c);
    }

    SECTION("gaussian curvature")
    {
        std::vector<float> gaussian_curvatures;
        for (auto v : vertices(bumpy)) {
            auto c = Euclid::gaussian_curvature(v, bumpy);
            gaussian_curvatures.push_back(c);
        }

        auto [vbeg, vend] = vertices(bumpy);
        auto curvatures = Euclid::gaussian_curvatures(bumpy);
        REQUIRE(Euclid::gaussian_curvature(*vbeg, bumpy) == curvatures[0]);
        REQUIRE(curvatures.size() == num_vertices(bumpy));

        std::vector<uint8_t> colors;
        Euclid::colormap(igl::COLOR_MAP_TYPE_JET, curvatures, colors, true);
        std::string fout(TMP_DIR);
        fout.append("bumpy_gaussian_curvature.ply");
        Euclid::write_ply<3>(
            fout, bpositions, nullptr, nullptr, &bindices, &colors);
    }

    SECTION("mean curvature w/ laplace beltrami operator")
    {
        auto laplacian = Euclid::cotangent_matrix(bumpy);
        auto mass = Euclid::mass_matrix(bumpy);
        Eigen::SparseMatrix<float> inv_mass;
        igl::invert_diag(mass, inv_mass);
        Eigen::MatrixXf hn = inv_mass * laplacian * bumpy_v;
        Eigen::VectorXf norms = hn.rowwise().norm();

        std::vector<float> mean_curvatures(hn.rows() * 3);
        auto nmax = norms.maxCoeff();
        nmax = 255.0 / nmax;
        for (int i = 0; i < hn.rows(); ++i) {
            mean_curvatures[i * 3 + 0] = norms(i) * nmax;
            mean_curvatures[i * 3 + 1] = 0.0f;
            mean_curvatures[i * 3 + 2] = 0.0f;
        }
        std::string fout(TMP_DIR);
        fout.append("bumpy_mean_curvature_lbo.ply");
        Euclid::write_ply<3>(
            fout, bpositions, nullptr, nullptr, &bindices, &mean_curvatures);
    }

    SECTION("mean curvature w/ graph laplacian operator")
    {
        auto [adj, degree] = Euclid::adjacency_matrix(bumpy);
        auto laplacian = degree - adj;
        Eigen::SparseMatrix<float> inv_degree;
        igl::invert_diag(degree, inv_degree);
        Eigen::MatrixXf hn = inv_degree * laplacian * bumpy_v;
        Eigen::VectorXf norms = hn.rowwise().norm();

        std::vector<float> mean_curvatures(hn.rows() * 3);
        auto nmax = norms.maxCoeff();
        nmax = 255.0 / nmax;
        for (int i = 0; i < hn.rows(); ++i) {
            mean_curvatures[i * 3 + 0] = norms(i) * nmax;
            mean_curvatures[i * 3 + 1] = 0.0f;
            mean_curvatures[i * 3 + 2] = 0.0f;
        }
        std::string fout(TMP_DIR);
        fout.append("bumpy_mean_curvature_gl.ply");
        Euclid::write_ply<3>(
            fout, bpositions, nullptr, nullptr, &bindices, &mean_curvatures);
    }
}


#pragma once

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

#include "hermiten_impl.hpp"
#include "poly_base.hpp"

namespace boltzmann {

/**
 * @brief Physicists' Hermite functions normalized
 *        This class evaluates
 *        \f$ h_j(x) exp(-x^2/2)\f$, where
 *         \f$ h_j(x)\f$ is the normalized physicists' Hermite polynomial, orthogonal wrt.
 *         weight \f$ exp(-x^21)\f$.
 *
 */
template <typename T>
class HermiteNW : public PolyBase<T>
{
 public:
  using typename PolyBase<T>::numeric_t;

 public:
  HermiteNW(unsigned int n);
  void compute(const std::vector<numeric_t>& x);

 private:
  using PolyBase<T>::Y_;
  using PolyBase<T>::n_;
};

// ----------------------------------------------------------------------
template <typename T>
HermiteNW<T>::HermiteNW(unsigned int n)
    : PolyBase<T>(n)
{ /* empty */
}

// ----------------------------------------------------------------------
template <typename T>
void
HermiteNW<T>::compute(const std::vector<numeric_t>& x)
{
  Y_.resize(boost::extents[n_ + 1][x.size()]);
  unsigned int N = x.size();

  std::vector<numeric_t> expw(x.size());
  for (unsigned int i = 0; i < N; ++i) {
    expw[i] = ::math::exp(-x[i] * x[i] * 1 / numeric_t(2));
  }

  // initalize l = 0
  for (unsigned int i = 0; i < N; ++i) {
    Y_[0][i] = boost::math::hermiten(0, x[i]) * expw[i];
    Y_[1][i] = boost::math::hermiten(1, x[i]) * expw[i];
  }

  for (unsigned int l = 1; l < n_; ++l) {
    //#pragma omp parallel for
    for (unsigned int i = 0; i < N; ++i) {
      Y_[l + 1][i] = boost::math::hermiten_next(l, x[i], Y_[l][i], Y_[l - 1][i]);
    }
  }
}

}  // end namespace boltzmann


// This scripts runs the simulations with four populations (Section 6.1)

#include <src/algorithms/neal2_algorithm.h>
#include <src/algorithms/semihdp_sampler.h>
#include <src/collectors/file_collector.h>
#include <src/collectors/memory_collector.h>
#include <src/includes.h>
#include <src/utils/rng.h>

#include <Eigen/Dense>
#include <chrono>
#include <stan/math/prim.hpp>
#include <vector>

using Eigen::MatrixXd;

MatrixXd generate_mixture(double m1, double s1, double m2, double s2, double w,
                          int n, std::mt19937_64& rng) {
  MatrixXd out(n, 1);
  for (int i = 0; i < n; i++) {
    if (stan::math::uniform_rng(0, 1, rng) < w) {
      out(i, 0) = stan::math::normal_rng(m1, s1, rng);
    } else {
      out(i, 0) = stan::math::normal_rng(m2, s2, rng);
    }
  }
  return out;
}

void run_semihdp2(const std::vector<MatrixXd> data, std::string chainfile,
                  std::string update_c = "full") {
  // compute overall mean
  double mu0 = std::accumulate(
      data.begin(), data.end(), 0,
      [&](int curr, const MatrixXd dat) { return curr + dat.sum(); });
  mu0 /= std::accumulate(
      data.begin(), data.end(), 0.0,
      [&](int curr, const MatrixXd dat) { return curr + dat.rows(); });
  auto hier = std::make_shared<NNIGHierarchy>();
  bayesmix::NNIGPrior hier_prior;
  hier_prior.mutable_fixed_values()->set_mean(mu0);
  hier_prior.mutable_fixed_values()->set_var_scaling(0.1);
  hier_prior.mutable_fixed_values()->set_shape(2.0);
  hier_prior.mutable_fixed_values()->set_scale(2.0);
  hier->get_mutable_prior()->CopyFrom(hier_prior);
  hier->initialize();

  // Collect pseudo priors
  std::vector<MemoryCollector> pseudoprior_collectors;
  pseudoprior_collectors.resize(data.size());
  bayesmix::DPPrior mix_prior;
  double totalmass = 1.0;
  mix_prior.mutable_fixed_value()->set_totalmass(totalmass);
#pragma omp parallel for
  for (int i = 0; i < data.size(); i++) {
    auto mixing = std::make_shared<DirichletMixing>();
    mixing->get_mutable_prior()->CopyFrom(mix_prior);
    mixing->set_num_components(5);
    auto hier = std::make_shared<NNIGHierarchy>();
    bayesmix::NNIGPrior hier_prior;
    hier_prior.mutable_fixed_values()->set_mean(data[i].mean());
    hier_prior.mutable_fixed_values()->set_var_scaling(0.1);
    hier_prior.mutable_fixed_values()->set_shape(2.0);
    hier_prior.mutable_fixed_values()->set_scale(2.0);
    hier->get_mutable_prior()->CopyFrom(hier_prior);

    Neal2Algorithm sampler;
    sampler.set_maxiter(2000);
    sampler.set_burnin(1000);
    sampler.set_mixing(mixing);
    sampler.set_data(data[i]);
    sampler.set_hierarchy(hier);
    sampler.run(&pseudoprior_collectors[i], false);
  }

  auto start = std::chrono::high_resolution_clock::now();
  int nburn = 5000;
  int niter = 5000;
  MemoryCollector collector;

  bayesmix::SemiHdpParams params;
  bayesmix::read_proto_from_file(
      "/home/mario/dev/bayesmix/resources/semihdp_params.asciipb", &params);
  params.set_rest_allocs_update(update_c);

  SemiHdpSampler sampler(data, hier, params);
  sampler.run(500, nburn, niter, 5, &collector, pseudoprior_collectors, true,
              200);
  auto end = std::chrono::high_resolution_clock::now();
  auto duration =
      std::chrono::duration_cast<std::chrono::seconds>(end - start).count();
  std::cout << "Finished running, duration: " << duration << std::endl;
  collector.write_to_file<bayesmix::SemiHdpState>(chainfile);
}

int main() {
  // Scenario VII
  std::vector<MatrixXd> data(100);

  auto& rng = bayesmix::Rng::Instance().get();

  for (int i = 0; i < 20; i++) {
    auto rng = bayesmix::Rng::Instance().get();
    data[i] = generate_mixture(-5, 1.0, 5, 1.0, 0.5, 100, rng);
  }
  for (int i = 20; i < 40; i++) {
    auto rng = bayesmix::Rng::Instance().get();
    data[i] = generate_mixture(-5.0, 1.0, 0.0, 1.0, 0.5, 100, rng);
  }
  for (int i = 40; i < 60; i++) {
    auto rng = bayesmix::Rng::Instance().get();
    data[i] = generate_mixture(0.0, 1.0, 5.0, 0.1, 0.5, 100, rng);
  }
  for (int i = 60; i < 80; i++) {
    auto rng = bayesmix::Rng::Instance().get();
    data[i] = generate_mixture(-10, 1.0, 0.0, 1.0, 0.5, 100, rng);
  }
  for (int i = 80; i < 100; i++) {
    auto rng = bayesmix::Rng::Instance().get();
    data[i] = generate_mixture(-10, 1.0, 0.0, 1.0, 0.1, 100, rng);
  }

  run_semihdp2(data,
               "/home/mario/PhD/exchangeability/semihdp-scripts/"
               "new_chains/s100.recordio",
               "metro_dist");
}


#define BOOST_TEST_MODULE test_onecellcadbug

#include <optional>

#include <boost/test/unit_test.hpp>

#include <smtrat-common/smtrat-common.h>
#include <smtrat-mcsat/smtrat-mcsat.h>

#include <carl/core/MultivariatePolynomial.h>
#include <carl/core/Variable.h>
#include <carl/formula/model/evaluation/ModelEvaluation.h>

#include <smtrat-mcsat/explanations/onecellcad/Explanation.h>

using namespace smtrat;

BOOST_AUTO_TEST_CASE(OneCellCadBug) {
	// Variable x cannot be assigned for x^2 + x^4 + b*x^3 <= 0 , x != 0 under b -> -1

	carl::Variable x = carl::freshRealVariable("x");
	carl::Variable b = carl::freshRealVariable("b");
  carl::Variables vars({x,b});

	FormulaT f1(ConstraintT((Poly(x)*x) + (Poly(x)*x*x*x) + (Poly(b)*x*x*x), carl::Relation::LEQ));
  FormulaT f2(ConstraintT(x, carl::Relation::NEQ));

	// generate explanation
  mcsat::Bookkeeping bookkeeping;
  bookkeeping.updateVariables(vars);
  bookkeeping.pushConstraint(f1);
  bookkeeping.pushConstraint(f2);
  bookkeeping.pushAssignment(b, Rational(-1), FormulaT(carl::FormulaType::TRUE));
  ::smtrat::mcsat::onecellcad::Explanation expl;
  auto explanation = expl(bookkeeping, x, FormulasT({f1,f2}));

  // proof that satisfying assignment exist
  Model model;
	model.assign(b, Rational(-5));
  model.assign(x, Rational(1));

  auto res = carl::model::evaluate(f1, model);
	BOOST_CHECK(res.isBool() && res.asBool());
  auto res1 = carl::model::evaluate(f2, model);
	BOOST_CHECK(res1.isBool() && res1.asBool());

  auto res2 = carl::model::evaluate(boost::get<FormulaT>(*explanation), model);
  BOOST_CHECK(res2.isBool());
  BOOST_CHECK(res2.asBool());
}


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT License.

#define _USE_MATH_DEFINES
#include "transforms.h"

#include <Eigen/Geometry>
#include <cmath>
#include <exception>
#include <iostream>

namespace scenepic
{
  namespace Transforms
  {
    Transform scale(float scale)
    {
      Transform matrix = Transform::Identity();
      matrix(0, 0) = matrix(1, 1) = matrix(2, 2) = scale;
      return matrix;
    }

    Transform scale(const Vector& scale)
    {
      Transform matrix = Transform::Identity();
      matrix(0, 0) = scale(0);
      matrix(1, 1) = scale(1);
      matrix(2, 2) = scale(2);
      return matrix;
    }

    Transform translate(const Vector& vec)
    {
      Transform matrix = Transform::Identity();
      matrix.topRightCorner(3, 1) = vec.transpose();
      return matrix;
    }

    Transform rotation_matrix_from_axis_angle(const Vector& axis, float angle)
    {
      if (std::abs(angle) < 0.0001)
      {
        return Transform::Identity();
      }

      float x = axis(0);
      float y = axis(1);
      float z = axis(2);
      float cos = std::cos(angle);
      float sin = std::sin(angle);
      Transform matrix;
      matrix << x * x + (1 - x * x) * cos, x * y * (1 - cos) - z * sin,
        x * z * (1 - cos) + y * sin, 0, x * y * (1 - cos) + z * sin,
        y * y + (1 - y * y) * cos, y * z * (1 - cos) - x * sin, 0,
        x * z * (1 - cos) - y * sin, z * y * (1 - cos) + x * sin,
        z * z + (1 - z * z) * cos, 0, 0, 0, 0, 1;
      return matrix;
    }

    Quaternion quaternion_from_axis_angle(const Vector& axis, float angle)
    {
      Vector norm_axis = axis.normalized();
      float half_sin = std::sin(angle * 0.5f);
      float half_cos = std::cos(angle * 0.5f);
      Quaternion quat(
        norm_axis(0) * half_sin,
        norm_axis(1) * half_sin,
        norm_axis(2) * half_sin,
        half_cos);
      return quat;
    }

    std::pair<Vector, float> axis_angle_to_align_x_to_axis(const Vector& axis)
    {
      std::pair<Vector, float> axis_angle;
      Vector norm_axis = axis.normalized();
      if (norm_axis(1) == 0 && norm_axis(2) == 0)
      {
        if (norm_axis(0) == -1)
        {
          axis_angle.first << 0, 1, 0;
          axis_angle.second = static_cast<float>(M_PI);
        }
        else
        {
          axis_angle.first << 1, 0, 0;
          axis_angle.second = 0;
        }
      }
      else
      {
        float rot_angle = std::acos(norm_axis(0));
        if (rot_angle == 0)
        {
          axis_angle.first << 1, 0, 0;
          axis_angle.second = 0;
        }
        else
        {
          axis_angle.first << 0, -norm_axis(2), norm_axis(1);
          axis_angle.first /= std::sqrt(
            norm_axis(2) * norm_axis(2) + norm_axis(1) * norm_axis(1));
          axis_angle.second = rot_angle;
        }
      }

      return axis_angle;
    }

    Quaternion quaternion_to_align_x_to_axis(const Vector& axis)
    {
      auto axis_angle = axis_angle_to_align_x_to_axis(axis);
      return quaternion_from_axis_angle(axis_angle.first, axis_angle.second);
    }

    Transform rotation_to_align_x_to_axis(const Vector& axis)
    {
      auto axis_angle = axis_angle_to_align_x_to_axis(axis);
      return rotation_matrix_from_axis_angle(
        axis_angle.first, axis_angle.second);
    }

    Transform rotation_about_x(float angle)
    {
      float cos = std::cos(angle);
      float sin = std::sin(angle);
      Transform matrix;
      matrix << 1, 0, 0, 0, 0, cos, -sin, 0, 0, sin, cos, 0, 0, 0, 0, 1;
      return matrix;
    }

    Transform rotation_about_y(float angle)
    {
      float cos = std::cos(angle);
      float sin = std::sin(angle);
      Transform matrix;
      matrix << cos, 0, sin, 0, 0, 1, 0, 0, -sin, 0, cos, 0, 0, 0, 0, 1;
      return matrix;
    }

    Transform rotation_about_z(float angle)
    {
      float cos = std::cos(angle);
      float sin = std::sin(angle);
      Transform matrix;
      matrix << cos, -sin, 0, 0, sin, cos, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
      return matrix;
    }

    Transform look_at_rotation(
      const Vector& center, const Vector& look_at, const Vector& up_dir)
    {
      Transform matrix = Transform::Identity();
      auto z_axis = (center - look_at).normalized();
      auto x_axis = (up_dir.cross(z_axis)).normalized();
      auto y_axis = (z_axis.cross(x_axis)).normalized();
      matrix.row(0).leftCols(3) = x_axis;
      matrix.row(1).leftCols(3) = y_axis;
      matrix.row(2).leftCols(3) = z_axis;
      return matrix;
    }

    Transform euler_angles_to_matrix(
      const Vector& euler_angles, const std::string& convention)
    {
      Transform matrix = Transform::Identity();
      for (auto i = 2; i >= 0; --i)
      {
        auto axis = convention[i];
        float angle = euler_angles(i);
        switch (axis)
        {
          case 'X':
          case 'x':
            matrix = rotation_about_x(angle) * matrix;
            break;

          case 'Y':
          case 'y':
            matrix = rotation_about_y(angle) * matrix;
            break;

          case 'Z':
          case 'z':
            matrix = rotation_about_z(angle) * matrix;
            break;

          default:
            throw std::invalid_argument("Invalid convention: " + convention);
        }
      }

      return matrix;
    }

    Transform quaternion_to_matrix(const Quaternion& quaternion)
    {
      float qw = quaternion.w();
      float qx = quaternion.x();
      float qy = quaternion.y();
      float qz = quaternion.z();
      float qx2 = qx * qx;
      float qy2 = qy * qy;
      float qz2 = qz * qz;
      Transform matrix;
      matrix << 1 - 2 * qy2 - 2 * qz2, 2 * qx * qy - 2 * qz * qw,
        2 * qx * qz + 2 * qy * qw, 0, 2 * qx * qy + 2 * qz * qw,
        1 - 2 * qx2 - 2 * qz2, 2 * qy * qz - 2 * qx * qw, 0,
        2 * qx * qz - 2 * qy * qw, 2 * qy * qz + 2 * qx * qw,
        1 - 2 * qx2 - 2 * qy2, 0, 0, 0, 0, 1;
      return matrix;
    }

    Quaternion quaternion_multiply(const Quaternion& a, const Quaternion& b)
    {
      float x = a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y();
      float y = a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z();
      float z = a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x();
      float w = a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z();
      return Quaternion(x, y, z, w);
    }

    Transform gl_projection(
      double fov_y_degrees, double aspect_ratio, double znear, double zfar)
    {
      double fov_y = (M_PI * fov_y_degrees / 180.0);
      double f = 1.0 / std::tan(fov_y / 2);
      float fx = static_cast<float>(f / aspect_ratio);
      float fy = static_cast<float>(f);
      double nf = 1.0 / (znear - zfar);
      float A = static_cast<float>((zfar + znear) * nf);
      float B = static_cast<float>(2 * zfar * znear * nf);

      Transform matrix;
      matrix << fx, 0, 0, 0, 0, fy, 0, 0, 0, 0, A, B, 0, 0, -1, 0;
      return matrix;
    }

    Transform gl_projection(
      const Intrinsic& camera_matrix,
      int width,
      int height,
      double znear,
      double zfar)
    {
      float K00 = camera_matrix(0, 0);
      float K01 = camera_matrix(0, 1);
      float K02 = camera_matrix(0, 2);
      float K11 = camera_matrix(1, 1);
      float K12 = camera_matrix(1, 2);
      float A = static_cast<float>((zfar + znear) / (znear - zfar));
      float B = static_cast<float>(2 * zfar * znear / (znear - zfar));
      Transform matrix;
      matrix << 2 * K00 / width, -2 * K01 / width, (width - 2 * K02) / width, 0,
        0, 2 * K11 / height, (2 * K12 - height) / height, 0, 0, 0, A, B, 0, 0,
        -1, 0;
      return matrix;
    }

    Transform gl_world_to_camera(const Extrinsic& extrinsic)
    {
      Transform camera_to_world =
        extrinsic * rotation_about_x(static_cast<float>(M_PI));
      return camera_to_world.inverse();
    }

  } // namespace Transforms
} // namespace scenepic