/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    Rot3M.cpp
 * @brief   Rotation (internal: 3*3 matrix representation*)
 * @author  Alireza Fathi
 * @author  Christian Potthast
 * @author  Frank Dellaert
 * @author  Richard Roberts
 */

#include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro

#ifndef GTSAM_USE_QUATERNIONS

#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h>
#include <boost/math/constants/constants.hpp>
#include <cmath>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
Rot3::Rot3() : rot_(I_3x3) {}

/* ************************************************************************* */
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
  Matrix3 R;
  R << col1, col2, col3;
  rot_ = SO3(R);
}

/* ************************************************************************* */
Rot3::Rot3(double R11, double R12, double R13, double R21, double R22,
           double R23, double R31, double R32, double R33) {
  Matrix3 R;
  R << R11, R12, R13, R21, R22, R23, R31, R32, R33;
  rot_ = SO3(R);
}

/* ************************************************************************* */
Rot3::Rot3(const gtsam::Quaternion& q) : rot_(q.toRotationMatrix()) {
}

/* ************************************************************************* */
Rot3 Rot3::Rx(double t) {
  double st = sin(t), ct = cos(t);
  return Rot3(
      1,  0,  0,
      0, ct,-st,
      0, st, ct);
}

/* ************************************************************************* */
Rot3 Rot3::Ry(double t) {
  double st = sin(t), ct = cos(t);
  return Rot3(
      ct, 0, st,
      0, 1,  0,
      -st, 0, ct);
}

/* ************************************************************************* */
Rot3 Rot3::Rz(double t) {
  double st = sin(t), ct = cos(t);
  return Rot3(
      ct,-st, 0,
      st, ct, 0,
      0,  0, 1);
}

/* ************************************************************************* */
// Considerably faster than composing matrices above !
Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx,
                  OptionalJacobian<3, 1> Hy, OptionalJacobian<3, 1> Hz) {
  double cx=cos(x),sx=sin(x);
  double cy=cos(y),sy=sin(y);
  double cz=cos(z),sz=sin(z);
  double ss_ = sx * sy;
  double cs_ = cx * sy;
  double sc_ = sx * cy;
  double cc_ = cx * cy;
  double c_s = cx * sz;
  double s_s = sx * sz;
  double _cs = cy * sz;
  double _cc = cy * cz;
  double s_c = sx * cz;
  double c_c = cx * cz;
  double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz;
  if (Hx) (*Hx) << 1, 0, 0;
  if (Hy) (*Hy) << 0, cx, -sx;
  if (Hz) (*Hz) << -sy, sc_, cc_;
  return Rot3(
      _cc,- c_s + ssc,  s_s + csc,
      _cs,  c_c + sss, -s_c + css,
      -sy,        sc_,        cc_
  );
}

/* ************************************************************************* */
Rot3 Rot3::normalized() const {
  /// Implementation from here: https://stackoverflow.com/a/23082112/1236990

  /// Essentially, this computes the orthogonalization error, distributes the
  /// error to the x and y rows, and then performs a Taylor expansion to
  /// orthogonalize.

  Matrix3 rot = rot_.matrix(), rot_orth;

  // Check if determinant is already 1.
  // If yes, then return the current Rot3.
  if (std::fabs(rot.determinant()-1) < 1e-12) return Rot3(rot_);

  Vector3 x = rot.block<1, 3>(0, 0), y = rot.block<1, 3>(1, 0);
  double error = x.dot(y);

  Vector3 x_ort = x - (error / 2) * y, y_ort = y - (error / 2) * x;
  Vector3 z_ort = x_ort.cross(y_ort);

  rot_orth.block<1, 3>(0, 0) = 0.5 * (3 - x_ort.dot(x_ort)) * x_ort;
  rot_orth.block<1, 3>(1, 0) = 0.5 * (3 - y_ort.dot(y_ort)) * y_ort;
  rot_orth.block<1, 3>(2, 0) = 0.5 * (3 - z_ort.dot(z_ort)) * z_ort;

  return Rot3(rot_orth);
}

/* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const {
  return Rot3(rot_*R2.rot_);
}

/* ************************************************************************* */
Matrix3 Rot3::transpose() const {
  return rot_.matrix().transpose();
}

/* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p,
    OptionalJacobian<3,3> H1,  OptionalJacobian<3,3> H2) const {
  if (H1) *H1 = rot_.matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
  if (H2) *H2 = rot_.matrix();
  return rot_.matrix() * p;
}

/* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) {
  return SO3::Logmap(R.rot_,H);
}

/* ************************************************************************* */
Rot3 Rot3::CayleyChart::Retract(const Vector3& omega, OptionalJacobian<3,3> H) {
  if (H) throw std::runtime_error("Rot3::CayleyChart::Retract Derivative");
  const double x = omega(0), y = omega(1), z = omega(2);
  const double x2 = x * x, y2 = y * y, z2 = z * z;
  const double xy = x * y, xz = x * z, yz = y * z;
  const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f;
  return Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f,
          (xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f,
          (xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f);
}

/* ************************************************************************* */
Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) {
  if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative");
  // Create a fixed-size matrix
  Matrix3 A = R.matrix();
  // Mathematica closed form optimization (procrastination?) gone wild:
  const double a = A(0, 0), b = A(0, 1), c = A(0, 2);
  const double d = A(1, 0), e = A(1, 1), f = A(1, 2);
  const double g = A(2, 0), h = A(2, 1), i = A(2, 2);
  const double di = d * i, ce = c * e, cd = c * d, fg = f * g;
  const double M = 1 + e - f * h + i + e * i;
  const double K = -4.0 / (cd * h + M + a * M - g * (c + ce) - b * (d + di - fg));
  const double x = a * f - cd + f;
  const double y = b * f - ce - c;
  const double z = fg - di - d;
  return K * Vector3(x, y, z);
}

/* ************************************************************************* */
Rot3 Rot3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) {
  static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
  if (mode == Rot3::EXPMAP) return Expmap(omega, H);
  if (mode == Rot3::CAYLEY) return CayleyChart::Retract(omega, H);
  else throw std::runtime_error("Rot3::Retract: unknown mode");
}

/* ************************************************************************* */
Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) {
  static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
  if (mode == Rot3::EXPMAP) return Logmap(R, H);
  if (mode == Rot3::CAYLEY) return CayleyChart::Local(R, H);
  else throw std::runtime_error("Rot3::Local: unknown mode");
}

/* ************************************************************************* */
Matrix3 Rot3::matrix() const {
  return rot_.matrix();
}

/* ************************************************************************* */
Point3 Rot3::r1() const { return Point3(rot_.matrix().col(0)); }

/* ************************************************************************* */
Point3 Rot3::r2() const { return Point3(rot_.matrix().col(1)); }

/* ************************************************************************* */
Point3 Rot3::r3() const { return Point3(rot_.matrix().col(2)); }

/* ************************************************************************* */
gtsam::Quaternion Rot3::toQuaternion() const {
  return gtsam::Quaternion(rot_.matrix());
}

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

} // namespace gtsam

#endif


#pragma once

#include "permutation.hxx"
#include <Eigen/OrderingMethods>
#include <vector>
#include <limits>

namespace LPMP {

    template<typename ADJACENCY_GRAPH>
    permutation minimum_degree_ordering(const ADJACENCY_GRAPH& adj, const size_t nr_vars)
    {
        MEASURE_FUNCTION_EXECUTION_TIME;
        Eigen::AMDOrdering<int> ordering;
        Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> perm(adj.size());
        Eigen::SparseMatrix<double> A(adj.size(), adj.size());
        std::vector< Eigen::Triplet<double> > adjacency_list;
        for(std::size_t i=0; i<adj.size(); ++i) {
            for(const std::size_t j : adj[i]) {
                assert(j <= std::numeric_limits<int>::max());
                adjacency_list.push_back({i,j,1.0});
                adjacency_list.push_back({i,j,1.0});
            }
        }
        A.setFromTriplets(adjacency_list.begin(), adjacency_list.end());
        ordering(A, perm);
        permutation o;
        for(std::size_t i=0; i<perm.indices().size(); ++i) {
            if (perm.indices()[i] < nr_vars)
                o.push_back(perm.indices()[i]);
        }
        assert(is_permutation(o.begin(), o.end()));
        return o;
    }

}


/*
Copyright (c) 2017 InversePalindrome
InPal - GrapherPanel.cpp
InversePalindrome.com
*/

#include "GrapherPanel.hpp"

#include <wx/sizer.h>
#include <wx/button.h>
#include <wx/choice.h>

#include <boost/algorithm/string/replace.hpp>
#include <boost/math/constants/constants.hpp>

#include <algorithm>

GrapherPanel::GrapherPanel(wxWindow* parent, MathDataDefault* mathData) :
    wxPanel(parent, wxID_ANY, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, "GrapherPanel"),
    mathData(mathData),
    graphText(new wxStaticText(this, wxID_ANY, "F(x) = ")),
    graphEntry(new wxTextCtrl(this, wxID_ANY, "", wxDefaultPosition, wxSize(550u, 0u)))
{
    SetBackgroundColour(wxColor(128u, 128u, 128u));

    mathData->graphData.plotWindow = new mpWindow(this, wxID_ANY, wxDefaultPosition, wxSize(100u, 100u), wxSUNKEN_BORDER);
    mathData->graphData.graphType = "Cartesian";
    mathData->graphData.minX = -10;
    mathData->graphData.maxX = 10;

    auto* topSizer = new wxBoxSizer(wxVERTICAL);
    auto* bottomSizer = new wxBoxSizer(wxHORIZONTAL);

    mathData->graphData.plotWindow->AddLayer(new mpScaleX("X", mpALIGN_CENTER));
    mathData->graphData.plotWindow->AddLayer(new mpScaleY("Y", mpALIGN_CENTER));
    mathData->graphData.plotWindow->SetMPScrollbars(false);
    mathData->graphData.plotWindow->EnableMousePanZoom(false);
    mathData->graphData.plotWindow->Fit(mathData->graphData.minX, mathData->graphData.maxX,
        mathData->graphData.minX, mathData->graphData.maxX);

    auto* graphButton = new wxButton(this, wxID_ANY, "Graph");

    auto& font = wxFont(16u, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);

    graphText->SetFont(font);
    graphEntry->SetFont(font);

    bottomSizer->Add(graphText, 0u, wxALL, 2u);
    bottomSizer->Add(graphEntry, 0u, wxALL | wxEXPAND, 2u);
    bottomSizer->Add(graphButton, 0u, wxALL, 5u);

    topSizer->Add(mathData->graphData.plotWindow, 1u, wxALL | wxEXPAND, 10u);
    topSizer->Add(bottomSizer, 0u, wxALL, 10u);

    topSizer->Fit(this);
    topSizer->SetSizeHints(this);

    SetSizer(topSizer);

    graphButton->Bind(wxEVT_LEFT_DOWN, &GrapherPanel::OnGraphButton, this);
}

void GrapherPanel::OnUpdateGraphType(wxCommandEvent& event)
{
    const auto& typeSelection = dynamic_cast<wxChoice*>(this->FindWindowById(wxID_APPLY))->GetStringSelection();

    if (typeSelection == "Cartesian")
    {
        this->graphText->SetLabel("F(x) = ");
    }
    else if (typeSelection == "Polar")
    {
        this->graphText->SetLabel("r(t) = ");
    }
}

void GrapherPanel::graphCartesian(std::vector<double>& xCoordinates, std::vector<double>& yCoordinates,
    std::size_t numberOfPoints, double minX, double maxX)
{
    for (double i = minX; i <= maxX; i += std::abs(maxX - minX) / numberOfPoints)
    {
        auto graphEquation = this->graphEntry->GetValue().ToStdString();

        boost::replace_all(graphEquation, "x", "(" + std::to_string(i) + ")");

        this->mathData->mathSolver.setTask(graphEquation);

        if (this->mathData->mathSolver.solve())
        {
            xCoordinates.push_back(i);
            yCoordinates.push_back(this->mathData->mathSolver.getValue());
        }
    }
}

void GrapherPanel::graphPolar(std::vector<double>& xCoordinates, std::vector<double>& yCoordinates, std::size_t numberOfPoints)
{
    const auto& pi = boost::math::constants::pi<double>();

    for (double i = 0.0; i <= 2.0 * pi; i += (2 * pi) / numberOfPoints)
    {
        auto graphEquation = this->graphEntry->GetValue().ToStdString();

        boost::replace_all(graphEquation, "t", "(" + std::to_string(i) + ")");

        this->mathData->mathSolver.setTask(graphEquation);

        if (mathData->mathSolver.solve())
        {
            xCoordinates.push_back(this->mathData->mathSolver.getValue() * std::cos(i));
            yCoordinates.push_back(this->mathData->mathSolver.getValue() * std::sin(i));
        }
    }
}

void GrapherPanel::OnGraphButton(wxMouseEvent& event)
{
    const std::size_t numberOfPoints = 2500u;

    std::vector<double> xCoordinates;
    std::vector<double> yCoordinates;

    double minX = this->mathData->graphData.minX;
    double maxX = this->mathData->graphData.maxX;

    if (mathData->graphData.graphType == "Cartesian")
    {
        this->graphCartesian(xCoordinates, yCoordinates, numberOfPoints, minX, maxX);
    }
    else if (mathData->graphData.graphType == "Polar")
    {
        this->graphPolar(xCoordinates, yCoordinates, numberOfPoints);
    }

    this->mathData->graphData.graphs.push_back(new mpFXYVector(this->graphEntry->GetValue().ToStdString()));
    this->mathData->graphData.graphs.back()->SetData(xCoordinates, yCoordinates);
    this->mathData->graphData.graphs.back()->SetContinuity(true);
    this->mathData->graphData.graphs.back()->ShowName(false);

    this->mathData->graphData.plotWindow->AddLayer(this->mathData->graphData.graphs.back());

    this->mathData->graphData.plotWindow->Fit();
}


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

// 程序以通常的包含文件开始，所有这些文件你现在应该都见过了。

#include <deal.II/base/utilities.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/affine_constraints.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/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/numerics/data_out.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/error_estimator.h>
#include <deal.II/numerics/solution_transfer.h>
#include <deal.II/numerics/matrix_tools.h>

#include <fstream>
#include <iostream>

// 然后照例将这个程序的所有内容放入一个命名空间，并将deal.II命名空间导入到我们将要工作的命名空间中。

namespace Step26
{
  using namespace dealii;
// @sect3{The HeatEquation class}

// 下一个部分是这个程序的主类的声明。它沿用了以前的例子中公认的路径。如果你看过 step-6 ，例如，这里唯一值得注意的是，我们需要建立两个矩阵（质量和拉普拉斯矩阵），并保存当前和前一个时间步骤的解。然后，我们还需要存储当前时间、时间步长和当前时间步长的编号。最后一个成员变量表示介绍中讨论的theta参数，它允许我们在一个程序中处理显式和隐式欧拉方法，以及Crank-Nicolson方法和其他通用方法。

// 就成员函数而言，唯一可能的惊喜是 refine_mesh 函数需要最小和最大的网格细化级别的参数。这样做的目的在介绍中已经讨论过了。

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

  private:
    void setup_system();
    void solve_time_step();
    void output_results() const;
    void refine_mesh(const unsigned int min_grid_level,
                     const unsigned int max_grid_level);

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

    AffineConstraints<double> constraints;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> mass_matrix;
    SparseMatrix<double> laplace_matrix;
    SparseMatrix<double> system_matrix;

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

    double       time;
    double       time_step;
    unsigned int timestep_number;

    const double theta;
  };

//  @sect3{Equation data}

// 在下面的类和函数中，我们实现了定义这个问题的各种数据（右手边和边界值），这些数据在这个程序中使用，我们需要函数对象。右手边的选择是在介绍的最后讨论的。对于边界值，我们选择零值，但这很容易在下面改变。

  template <int dim>
  class RightHandSide : public Function<dim>
  {
  public:
    RightHandSide()
      : Function<dim>()
      , period(0.2)
    {}

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

  private:
    const double period;
  };

  template <int dim>
  double RightHandSide<dim>::value(const Point<dim> & p,
                                   const unsigned int component) const
  {
    (void)component;
    AssertIndexRange(component, 1);
    Assert(dim == 2, ExcNotImplemented());

    const double time = this->get_time();
    const double point_within_period =
      (time / period - std::floor(time / period));

    if ((point_within_period >= 0.0) && (point_within_period <= 0.2))
      {
        if ((p[0] > 0.5) && (p[1] > -0.5))
          return 1;
        else
          return 0;
      }
    else if ((point_within_period >= 0.5) && (point_within_period <= 0.7))
      {
        if ((p[0] > -0.5) && (p[1] > 0.5))
          return 1;
        else
          return 0;
      }
    else
      return 0;
  }

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

  template <int dim>
  double BoundaryValues<dim>::value(const Point<dim> & /*p*/,
                                    const unsigned int component) const
  {
    (void)component;
    Assert(component == 0, ExcIndexRange(component, 0, 1));
    return 0;
  }

//  @sect3{The HeatEquation implementation}

// 现在是实现主类的时候了。让我们从构造函数开始，它选择了一个线性元素，一个时间步长为1/500的常数（记得上面把右边的源的一个周期设置为0.2，所以我们用100个时间步长来解决每个周期），并通过设置  $\theta=1/2$  选择了Crank Nicolson方法.

  template <int dim>
  HeatEquation<dim>::HeatEquation()
    : fe(1)
    , dof_handler(triangulation)
    , time_step(1. / 500)
    , theta(0.5)
  {}

//  @sect4{HeatEquation::setup_system}

// 下一个函数是设置DoFHandler对象，计算约束，并将线性代数对象设置为正确的大小。我们还在这里通过简单地调用库中的两个函数来计算质量和拉普拉斯矩阵。

// 注意我们在组装矩阵时不考虑悬挂节点的约束（两个函数都有一个AffineConstraints参数，默认为一个空对象）。这是因为我们要在结合当前时间步长的矩阵后，在run()中浓缩约束。

  template <int dim>
  void HeatEquation<dim>::setup_system()
  {
    dof_handler.distribute_dofs(fe);

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

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

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

    sparsity_pattern.copy_from(dsp);

    mass_matrix.reinit(sparsity_pattern);
    laplace_matrix.reinit(sparsity_pattern);
    system_matrix.reinit(sparsity_pattern);

    MatrixCreator::create_mass_matrix(dof_handler,
                                      QGauss<dim>(fe.degree + 1),
                                      mass_matrix);
    MatrixCreator::create_laplace_matrix(dof_handler,
                                         QGauss<dim>(fe.degree + 1),
                                         laplace_matrix);

    solution.reinit(dof_handler.n_dofs());
    old_solution.reinit(dof_handler.n_dofs());
    system_rhs.reinit(dof_handler.n_dofs());
  }
// @sect4{HeatEquation::solve_time_step}

// 下一个函数是解决单个时间步骤的实际线性系统的函数。这里没有什么值得惊讶的。

  template <int dim>
  void HeatEquation<dim>::solve_time_step()
  {
    SolverControl            solver_control(1000, 1e-8 * system_rhs.l2_norm());
    SolverCG<Vector<double>> cg(solver_control);

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

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

    constraints.distribute(solution);

    std::cout << "     " << solver_control.last_step() << " CG iterations."
              << std::endl;
  }

//  @sect4{HeatEquation::output_results}

// 在生成图形输出方面也没有什么新东西，只是我们告诉DataOut对象当前的时间和时间步长是多少，以便将其写入输出文件中。

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

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

    data_out.build_patches();

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

    const std::string filename =
      "solution-" + Utilities::int_to_string(timestep_number, 3) + ".vtk";
    std::ofstream output(filename);
    data_out.write_vtk(output);
  }
// @sect4{HeatEquation::refine_mesh}

// 这个函数是程序中最有趣的部分。它负责自适应网格细化的工作。这个函数执行的三个任务是：首先找出需要细化/粗化的单元，然后实际进行细化，最后在两个不同的网格之间传输解向量。第一个任务是通过使用成熟的凯利误差估计器来实现的。第二项任务是实际进行再细化。这也只涉及到基本的函数，例如 refine_and_coarsen_fixed_fraction ，它可以细化那些具有最大估计误差的单元，这些误差加起来占60%，并粗化那些具有最小误差的单元，这些单元加起来占40%的误差。请注意，对于像当前这样的问题，即有事发生的区域正在四处移动，我们希望积极地进行粗化，以便我们能够将单元格移动到有必要的地方。

// 正如在介绍中已经讨论过的，太小的网格会导致太小的时间步长，而太大的网格会导致太小的分辨率。因此，在前两个步骤之后，我们有两个循环，将细化和粗化限制在一个允许的单元范围内。

  template <int dim>
  void HeatEquation<dim>::refine_mesh(const unsigned int min_grid_level,
                                      const unsigned int max_grid_level)
  {
    Vector<float> estimated_error_per_cell(triangulation.n_active_cells());

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

    GridRefinement::refine_and_coarsen_fixed_fraction(triangulation,
                                                      estimated_error_per_cell,
                                                      0.6,
                                                      0.4);

    if (triangulation.n_levels() > max_grid_level)
      for (const auto &cell :
           triangulation.active_cell_iterators_on_level(max_grid_level))
        cell->clear_refine_flag();
    for (const auto &cell :
         triangulation.active_cell_iterators_on_level(min_grid_level))
      cell->clear_coarsen_flag();

// 上面这两个循环略有不同，但这很容易解释。在第一个循环中，我们没有调用  triangulation.end()  ，而是调用  triangulation.end_active(max_grid_level)  。这两个调用应该产生相同的迭代器，因为迭代器是按级别排序的，不应该有任何级别高于 max_grid_level 的单元格。事实上，这段代码确保了这种情况的发生。

// 作为网格细化的一部分，我们需要将旧的网格中的解向量转移到新的网格中。为此，我们使用了SolutionTransfer类，我们必须准备好需要转移到新网格的解向量（一旦完成细化，我们将失去旧的网格，所以转移必须与细化同时发生）。在我们调用这个函数的时候，我们将刚刚计算出解决方案，所以我们不再需要old_solution变量（它将在网格被细化后被解决方案覆盖，也就是在时间步长结束时；见下文）。换句话说，我们只需要一个求解向量，并将其复制到一个临时对象中，当我们进一步向下调用 setup_system() 时，它就不会被重置。

// 因此，我们将一个SolutionTransfer对象附加到旧的DoF处理程序中，以初始化它。然后，我们准备好三角形和数据向量，以便进行细化（按照这个顺序）。

    SolutionTransfer<dim> solution_trans(dof_handler);

    Vector<double> previous_solution;
    previous_solution = solution;
    triangulation.prepare_coarsening_and_refinement();
    solution_trans.prepare_for_coarsening_and_refinement(previous_solution);

// 现在一切都准备好了，所以进行细化并在新网格上重新创建DoF结构，最后在 setup_system 函数中初始化矩阵结构和新的向量。接下来，我们实际执行从旧网格到新网格的插值解。最后一步是对解向量应用悬空节点约束，即确保位于悬空节点上的自由度值，使解是连续的。这是必要的，因为SolutionTransfer只对单元格进行局部操作，不考虑邻域。

    triangulation.execute_coarsening_and_refinement();
    setup_system();

    solution_trans.interpolate(previous_solution, solution);
    constraints.distribute(solution);
  }

//  @sect4{HeatEquation::run}

// 这是程序的主要驱动，我们在这里循环所有的时间步骤。在函数的顶部，我们通过重复第一个时间步长，设置初始全局网格细化的数量和自适应网格细化的初始周期数量。然后，我们创建一个网格，初始化我们要处理的各种对象，设置一个标签，说明我们在重新运行第一个时间步长时应该从哪里开始，并将初始解插值到网格上（我们在这里选择了零函数，当然，我们可以用更简单的方法，直接将解向量设置为零）。我们还输出一次初始时间步长。

//  @note  如果你是一个有经验的程序员，你可能会对我们在这段代码中使用 goto 语句感到吃惊   goto 语句现在已经不是特别受人欢迎了，因为计算机科学界的大师之一Edsgar Dijkstra在1968年写了一封信，叫做 "Go To Statement considered harmful"（见here）。这段代码的作者全心全意地赞同这一观念。  goto 是难以理解的。事实上，deal.II几乎不包含任何出现的情况：不包括基本上是从书本上转录的代码，也不计算重复的代码片断，在写这篇笔记时，大约60万行代码中有3个位置；我们还在4个教程程序中使用它，其背景与这里完全相同。与其在这里试图证明这种情况的出现，不如先看看代码，我们在函数的最后再来讨论这个问题。

  template <int dim>
  void HeatEquation<dim>::run()
  {
    const unsigned int initial_global_refinement       = 2;
    const unsigned int n_adaptive_pre_refinement_steps = 4;

    GridGenerator::hyper_L(triangulation);
    triangulation.refine_global(initial_global_refinement);

    setup_system();

    unsigned int pre_refinement_step = 0;

    Vector<double> tmp;
    Vector<double> forcing_terms;

  start_time_iteration:

    time            = 0.0;
    timestep_number = 0;

    tmp.reinit(solution.size());
    forcing_terms.reinit(solution.size());

    VectorTools::interpolate(dof_handler,
                             Functions::ZeroFunction<dim>(),
                             old_solution);
    solution = old_solution;

    output_results();

// 然后我们开始主循环，直到计算的时间超过我们的结束时间0.5。第一个任务是建立我们需要在每个时间步骤中解决的线性系统的右手边。回顾一下，它包含项 $MU^{n-1}-(1-\theta)k_n AU^{n-1}$  。我们把这些项放到变量system_rhs中，借助于一个临时矢量。

    while (time <= 0.5)
      {
        time += time_step;
        ++timestep_number;

        std::cout << "Time step " << timestep_number << " at t=" << time
                  << std::endl;

        mass_matrix.vmult(system_rhs, old_solution);

        laplace_matrix.vmult(tmp, old_solution);
        system_rhs.add(-(1 - theta) * time_step, tmp);

// 第二块是计算源项的贡献。这与术语  $k_n \left[ (1-\theta)F^{n-1} + \theta F^n \right]$  相对应。下面的代码调用  VectorTools::create_right_hand_side  来计算向量  $F$  ，在这里我们在评估之前设置了右侧（源）函数的时间。这一切的结果最终都在forcing_terms变量中。

        RightHandSide<dim> rhs_function;
        rhs_function.set_time(time);
        VectorTools::create_right_hand_side(dof_handler,
                                            QGauss<dim>(fe.degree + 1),
                                            rhs_function,
                                            tmp);
        forcing_terms = tmp;
        forcing_terms *= time_step * theta;

        rhs_function.set_time(time - time_step);
        VectorTools::create_right_hand_side(dof_handler,
                                            QGauss<dim>(fe.degree + 1),
                                            rhs_function,
                                            tmp);

        forcing_terms.add(time_step * (1 - theta), tmp);

// 接下来，我们将强迫项加入到来自时间步长的强迫项中，同时建立矩阵 $M+k_n\theta A$ ，我们必须在每个时间步长中进行反转。这些操作的最后一块是消除线性系统中悬挂的节点约束自由度。

        system_rhs += forcing_terms;

        system_matrix.copy_from(mass_matrix);
        system_matrix.add(theta * time_step, laplace_matrix);

        constraints.condense(system_matrix, system_rhs);

// 在解决这个问题之前，我们还需要做一个操作：边界值。为此，我们创建一个边界值对象，将适当的时间设置为当前时间步长的时间，并像以前多次那样对其进行评估。其结果也被用来在线性系统中设置正确的边界值。

        {
          BoundaryValues<dim> boundary_values_function;
          boundary_values_function.set_time(time);

          std::map<types::global_dof_index, double> boundary_values;
          VectorTools::interpolate_boundary_values(dof_handler,
                                                   0,
                                                   boundary_values_function,
                                                   boundary_values);

          MatrixTools::apply_boundary_values(boundary_values,
                                             system_matrix,
                                             solution,
                                             system_rhs);
        }

// 有了这些，我们要做的就是解决这个系统，生成图形数据，以及......

        solve_time_step();

        output_results();

// ...负责网格的细化。在这里，我们要做的是：(i)在求解过程的最开始，细化所要求的次数，之后我们跳到顶部重新开始时间迭代，(ii)之后每隔五步细化一次。

// 时间循环和程序的主要部分以开始进入下一个时间步骤结束，将old_solution设置为我们刚刚计算出的解决方案。

        if ((timestep_number == 1) &&
            (pre_refinement_step < n_adaptive_pre_refinement_steps))
          {
            refine_mesh(initial_global_refinement,
                        initial_global_refinement +
                          n_adaptive_pre_refinement_steps);
            ++pre_refinement_step;

            tmp.reinit(solution.size());
            forcing_terms.reinit(solution.size());

            std::cout << std::endl;

            goto start_time_iteration;
          }
        else if ((timestep_number > 0) && (timestep_number % 5 == 0))
          {
            refine_mesh(initial_global_refinement,
                        initial_global_refinement +
                          n_adaptive_pre_refinement_steps);
            tmp.reinit(solution.size());
            forcing_terms.reinit(solution.size());
          }

        old_solution = solution;
      }
  }
} // namespace Step26

// 现在你已经看到了这个函数的作用，让我们再来看看  goto  的问题。从本质上讲，代码所做的事情是这样的。
// @code
//    void run ()
//    {
//      initialize;
//    start_time_iteration:
//      for (timestep=1...)
//      {
//         solve timestep;
//         if (timestep==1 && not happy with the result)
//         {
//           adjust some data structures;
//           goto start_time_iteration; simply try again
//         }
//         postprocess;
//      }
//    }
//  @endcode
//  这里，"对结果满意 "的条件是我们想保留当前的网格，还是宁愿细化网格并在新网格上重新开始。我们当然可以用下面的方法来取代  goto  的使用。
//  @code
//    void run ()
//    {
//      initialize;
//      while (true)
//      {
//         solve timestep;
//         if (not happy with the result)
//            adjust some data structures;
//         else
//            break;
//      }
//      postprocess;

//      for (timestep=2...)
//      {
//         solve timestep;
//         postprocess;
//      }
//    }
//  @endcode
//  这样做的好处是摆脱了 goto ，但缺点是必须在两个不同的地方重复实现 "解算时间步长 "和 "后处理 "操作的代码。这可以通过将这些部分的代码（在上面的实际实现中是相当大的块）放到自己的函数中来解决，但是一个带有 break 语句的 while(true) 循环并不真的比 goto 容易阅读或理解。

// 最后，人们可能会简单地同意，in general 。
// goto  语句是个坏主意，但要务实地指出，在某些情况下，它们可以帮助避免代码重复和尴尬的控制流。这可能就是其中之一，它与Steve McConnell在他关于良好编程实践的优秀书籍 "Code Complete"  @cite CodeComplete 中采取的立场一致（见 step-1 的介绍中提到的这本书），该书花了惊人的10页来讨论一般的 goto 问题。

//  @sect3{The main function}

// 走到这一步，这个程序的主函数又没有什么好讨论的了：它看起来就像自 step-6 以来的所有此类函数一样。

int main()
{
  try
    {
      using namespace Step26;

      HeatEquation<2> heat_equation_solver;
      heat_equation_solver.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;
}


/* Boost numeric test of the runge kutta steppers test file

 Copyright 2012 Mario Mulansky
 Copyright 2012 Karsten Ahnert

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

// disable checked iterator warning for msvc
#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE numeric_runge_kutta

#include <iostream>
#include <cmath>

#include <boost/array.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/vector.hpp>

#include <boost/numeric/odeint.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;
namespace mpl = boost::mpl;

typedef double value_type;

typedef boost::array< double , 1 > state_type;

// harmonic oscillator, analytic solution x[0] = sin( t )
struct osc
{
    void operator()( const state_type &x, const state_type &v, state_type &a,
                     const double t ) const
    {
        a[0] = -x[0];
    }
};

BOOST_AUTO_TEST_SUITE( velocity_verlet_test )

BOOST_AUTO_TEST_CASE( numeric_velocity_verlet_test )
{

    velocity_verlet<state_type> stepper;
    const int steps = 10;
    // order of the error is order of approximation + 1
    const int o = stepper.order() + 1;

    const state_type x0 = {{ 0.0 }};
    const state_type v0 = {{ 1.0 }};
    state_type x = x0;
    state_type v = v0;
    const double t = 0.0;
    /* do a first step with dt=0.1 to get an estimate on the prefactor of the error dx = f * dt^(order+1) */
    double dt = 0.5;
    for ( int step = 0; step < steps; ++step )
    {
        stepper.do_step(
            osc(), std::make_pair( boost::ref( x ), boost::ref( v ) ), t, dt );
    }
    const double f = steps * std::abs( sin( steps * dt ) - x[0] ) /
                     std::pow( dt, o ); // upper bound

    std::cout << o << " , " << f << std::endl;

    /* as long as we have errors above machine precision */
    while( f*std::pow( dt , o ) > 1E-16 )
    {
        x = x0;
        v = v0;
        stepper.reset();
        for ( int step = 0; step < steps; ++step )
        {
            stepper.do_step( osc() , std::make_pair(boost::ref(x), boost::ref(v)) , t , dt );
        }
        std::cout << "Testing dt=" << dt << std::endl;
        BOOST_CHECK_LT( std::abs( sin( steps * dt ) - x[0] ),
                        f * std::pow( dt, o ) );
        dt *= 0.5;
    }
};

BOOST_AUTO_TEST_SUITE_END()


//!
//! @file       matrix.cpp
//! @brief      implementing functions for matrix operations in plaintext
//!
//! @author     Miran Kim
//! @date       Dec. 1, 2017
//!

#include <iostream>
#include <stdio.h>
#include <vector>
#include <sys/time.h>

#include <cmath>
#include <map>
#include <math.h>  // pow
#include <sys/time.h>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <cstdlib>
#include <fstream>
#include <sstream>
#include <string>
#include <vector>
#include "math.h"
#include <cassert>
#include <random>
#include <string>
#include <iomanip>

#include <NTL/xdouble.h>
#include <NTL/ZZ.h>
#include "NTL/RR.h"
#include <NTL/ZZX.h>
#include "NTL/mat_RR.h"
#include "NTL/vec_RR.h"

#include "matrix.h"

//!@ Input: vec_RR
//!@ Function: print the vector
//!@ If print_size = 0, then print out all the components of an input vector

void printRvector(vec_RR& vec, long print_size){
    long len;

    if(print_size == 0){
        len = vec.length();
    }
    else{
        len = print_size;
    }

    cout << "   [" ;
    for(int i = 0; i < len; ++i){
        cout << " " << vec[i] << ((i != len - 1) ? "\t" : "]\n");
    }
}

//!@ Input: RR-matrix
//!@ Function: print the matrix
void printRmatrix(Mat<RR>& mat, const long print_size){
    long rlen, clen;

    if(print_size == 0){
        rlen = mat.NumRows();
        clen = mat.NumCols();
    }
    else{
        rlen = print_size;
        clen = print_size;
    }

    for(int i = 0; i< rlen; ++i){
        cout << "   [";
        for(int j = 0; j < clen; ++j){
            cout << mat[i][j] << ((j != clen - 1) ? "\t" : "]\n");
        }
    }
}

//!@ Input: A and B
//!@ Function: return the maximum norm of the difference of two input matrices A and B
RR getError(mat_RR Amat, mat_RR Bmat, long nrows, long ncols){
    RR ret = to_RR("0");

    for(long i = 0; i < nrows; ++i){
        for(long j = 0; j < ncols; ++j){
            RR temp = abs(Amat[i][j]-Bmat[i][j]);
            if (ret < temp){
                ret = temp;
            }
            if(temp > 1e-2){
                cout << "(" << i << "," << j  << ") = " << Amat[i][j] << ", " << Bmat[i][j]<< endl;
            }
        }
    }
    return ret;
}


/*
 ___ ___ __     __ ____________
|   |   |  |   |__|__|__   ___/  Ubiquitous Internet @ IIT-CNR
|   |   |  |  /__/  /  /  /      Stateful FaaS Model Latency Simulator
|   |   |  |/__/  /   /  /       https://github.com/ccicconetti/markovsim/
|_______|__|__/__/   /__/

Licensed under the MIT License <http://opensource.org/licenses/MIT>.
Copyright (c) 2021 Claudio Cicconetti <https://ccicconetti.github.io/>

Permission is hereby  granted, free of charge, to any  person obtaining a copy
of this software and associated  documentation files (the "Software"), to deal
in the Software  without restriction, including without  limitation the rights
to  use, copy,  modify, merge,  publish, distribute,  sublicense, and/or  sell
copies  of  the Software,  and  to  permit persons  to  whom  the Software  is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE  IS PROVIDED "AS  IS", WITHOUT WARRANTY  OF ANY KIND,  EXPRESS OR
IMPLIED,  INCLUDING BUT  NOT  LIMITED TO  THE  WARRANTIES OF  MERCHANTABILITY,
FITNESS FOR  A PARTICULAR PURPOSE AND  NONINFRINGEMENT. IN NO EVENT  SHALL THE
AUTHORS  OR COPYRIGHT  HOLDERS  BE  LIABLE FOR  ANY  CLAIM,  DAMAGES OR  OTHER
LIABILITY, WHETHER IN AN ACTION OF  CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE  OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

/*
Compute the average latency of a pool of FaaS clients, where:
- one group is assigned one stateful container each;
- another group of clients share a pool of stateless containers.
*/

#include "Support/chrono.h"
#include "Support/glograii.h"

#include <boost/program_options.hpp>

#include <glog/logging.h>

#include <cassert>
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <iostream>

namespace po = boost::program_options;

double erlang_c(size_t aWorkers, double aLoad) {
  assert(aWorkers > 0);
  assert(aLoad > 0);

  //            A
  // return ---------
  //          A + B

  // compute A
  double myFact = 1.0; // (aWorkers-1) * (aWorkers-2) * ... * 2 * 1
  for (size_t i = 2; i < aWorkers; i++) {
    myFact *= i;
  }
  double A = std::pow(aLoad, aWorkers) / (myFact * (aWorkers - aLoad));

  // compute B
  double B = 0;
  double myCurFact = 1.0;
  for (size_t i = 0; i < aWorkers; i++) {
    if (i > 0) {
      myCurFact *= i;
    }
    B += std::pow(aLoad, i) / myCurFact;
  }

  return A / (A + B);
}

int main(int argc, char *argv[]) {
  uiiit::support::GlogRaii myGlogRaii(argv[0]);

  size_t N_k; // number of clients
  size_t C_k; // number of containers
  double inv_mu_F;
  double inv_mu_L;
  double lambda_k;

  std::string myOutput;

#ifndef NDEBUG
  assert(std::abs(erlang_c(40, 36) - 0.41156) < 0.0001);
  assert(std::abs(erlang_c(40, 27) - 0.01272) < 0.0001);
  assert(std::abs(erlang_c(40, 18) - 0.0000055155) < 0.0000000001);
#endif

  po::options_description myDesc("Allowed options");
  // clang-format off
  myDesc.add_options()
    ("help,h", "produce help message")
    ("arrival-rate",
     po::value<double>(&lambda_k)->default_value(0.075),
     "Arrival rate, in Hz.")
    ("containers",
     po::value<size_t>(&C_k)->default_value(40),
     "Number of containers")
    ("clients",
     po::value<size_t>(&N_k)->default_value(70),
     "Number of clients")
    ("service-time-full",
     po::value<double>(&inv_mu_F)->default_value(1.0),
     "Service time for clients assigned a dedicated container, in s.")
    ("service-time-less",
     po::value<double>(&inv_mu_L)->default_value(3.0),
     "Service time for clients sharing a pool of non-dedicated containers, in s.")
    ("output",
     po::value<std::string>(&myOutput)->default_value("out.dat"),
     "Output file.")
    ;
  // clang-format on

  try {
    po::variables_map myVarMap;
    po::store(po::parse_command_line(argc, argv, myDesc), myVarMap);
    po::notify(myVarMap);

    if (myVarMap.count("help")) {
      std::cout << myDesc << std::endl;
      return EXIT_FAILURE;
    }

    if (inv_mu_F <= 0) {
      throw std::runtime_error("Invalid service time (full): " +
                               std::to_string(inv_mu_F));
    }
    double mu_F = 1.0 / inv_mu_F;
    if (inv_mu_L <= 0) {
      throw std::runtime_error("Invalid service time (less): " +
                               std::to_string(inv_mu_L));
    }
    double mu_L = 1.0 / inv_mu_L;

    std::ofstream myOutfile(myOutput);
    if (not myOutfile) {
      throw std::runtime_error("Could not open file: " + myOutput);
    }

    // n_F is the number of clients with dedicated containers
    // it ranges from 0 (all containers are shared)
    // to C_k-1 (only one container is shared)
    for (size_t n_F = 0; n_F < C_k; n_F++) {
      // number of clients associated to a pool of shared stateless containers
      auto n_L = N_k - n_F;

      // total load of clients associated to a pool of shared containers
      auto lambda_L = lambda_k * n_L;

      // number of shared stateless containers
      assert(C_k > n_F);
      auto C_L = C_k - n_F;

      // average latency of clients with a dedicated container
      double L_F = 1.0 / (mu_F - lambda_k);

      // utilisation of dedicated containers
      auto rho_F = lambda_k / mu_F;

      // utilisation of the shared pool of containers
      auto rho_L = lambda_L / (mu_L * C_L);

      VLOG(1) << "n_F = " << n_F << ", n_L = " << n_L << ", C_L " << C_L
              << ", mu_F = " << mu_F << ", mu_L = " << mu_L;

      // check stability
      if (rho_F >= 1.0 or rho_L >= 1.0) {
        VLOG(1) << "rho_F = " << rho_F << ", rho_L = " << rho_L
                << ": system unstable";
        continue;
      }

      // average latency of clients associated to a pool of shared containers
      auto L_L =
          n_L > 0 ? (erlang_c(C_L, lambda_L / mu_L) / (mu_L * C_L - lambda_L) +
                     inv_mu_L)
                  : 0.0;

      // average system latency
      double L = (n_F * L_F + n_L * L_L) / N_k;

      myOutfile << n_F << ' ' << L << '\n';
    }

    return EXIT_SUCCESS;

  } catch (const std::exception &aErr) {
    LOG(ERROR) << "Exception caught: " << aErr.what();

  } catch (...) {
    LOG(ERROR) << "Unknown exception caught";
  }

  return EXIT_FAILURE;
}


/*
 * utilities_cumtrapz_test.cpp Test fixtures for the cumtrapz function
 *
 * Author:                   Tom Clark (thclark @ github)
 *
 * Copyright (c) 2016-9 Octue Ltd. All Rights Reserved.
 *
 */

#include "gtest/gtest.h"
#include <Eigen/Dense>

#include "utilities/cumtrapz.h"

using namespace utilities;

class CumtrapzTest : public ::testing::Test {};

TEST_F(CumtrapzTest, test_colwise_cumtrapz_1_row) {

    // Test that zeros are returned when trying to colwise integrate an array with 1 row
    Eigen::ArrayXXf integrand(1, 4);
    integrand << 1, 2, 6, 9;
    Eigen::ArrayXXf integral_correct(1, 4);
    integral_correct << 0, 0, 0, 0;
    Eigen::ArrayXXf integral = cumtrapz(integrand);
    EXPECT_EQ(integral.matrix(), integral_correct.matrix());

}

TEST_F(CumtrapzTest, test_colwise_cumtrapz_2_rows) {

    // Test that uniform spaced integration works on an array with 2 rows
    Eigen::ArrayXXf integrand(2,6);
    integrand << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0,
        7.0, 8.0, 9.0, 10.0, 11.0, 12.0;
    Eigen::ArrayXXf integral_correct(2,6);
    integral_correct << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0;
    Eigen::ArrayXXf integral = cumtrapz(integrand);
    EXPECT_EQ(integral.matrix(), integral_correct.matrix());

}

TEST_F(CumtrapzTest, test_colwise_cumtrapz_6_rows) {

    Eigen::ArrayXXf integrand(6,1);
    integrand << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0;

    Eigen::ArrayXXf integral_correct(6,1);
    integral_correct<< 0.0, 1.5, 4.0, 7.5, 12.0, 17.5;

    Eigen::ArrayXXf integral = cumtrapz(integrand);
    EXPECT_EQ(integral.matrix(), integral_correct.matrix());

}

TEST_F(CumtrapzTest, test_colwise_nonuniform_cumtrapz) {

    // Test that non-uniform spacing works with an n x 1 spacing array
    Eigen::ArrayXXf spacing(3, 1);
    spacing << 2,
        5,
        7;
    Eigen::ArrayXXf integrand(3,4);
    integrand << 1, 2, 6, 9,
        3, 1, 7, 2,
        4, 8, 3, 1;
    Eigen::ArrayXXf integral_correct(3,4);
    integral_correct << 0.0, 0.0, 0.0, 0.0,
                        6.0, 4.5, 19.5, 16.5,
                        13.0, 13.5, 29.5, 19.5;
    Eigen::ArrayXXf integral(3,4);
    cumtrapz(integral, spacing, integrand);
    EXPECT_EQ(integral.matrix(), integral_correct.matrix());

}


#include "pch.h"

// Slow, but simple csv parsing lib
#include "include/rapidcsv/rapidcsv.h"

// Boost matrix
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/range/algorithm.hpp>

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

////------------------------------COMMON OPERATIONS-----------------------------//

template <class T>
pair<T, T> getMatrixMinMax(matrix<T> * x)  // Could be changed to boost vector minMax
{
	pair<T, T> minMax;
	minMax.first = (x)->operator()(0, 0); // Min
	minMax.second = (x)->operator()(0, 0); // Max

	for (int m = 0; m < (x)->size1(); ++m)
		for (int n = 0; n < (x)->size2(); n++)
		{
			T value = (x)->operator()(m, n);
			if (value < minMax.first)
				minMax.first = value;
			if (value > minMax.second)
				minMax.second = value;

		}
	return minMax;
}

template <class T>
void normlizeMatrix(matrix<T> * x) {

	pair<T, T> minMax = getMatrixMinMax<T>(x);
	T range = minMax.second - minMax.first;
	for (int m = 0; m < (x)->size1(); ++m)
		for (int n = 0; n < (x)->size2(); n++)
		{
			T old_value = (x)->operator()(m, n);
			(x)->operator()(m, n) = (old_value - minMax.first) / range;
		}
}

template <class T>
void unsignMatrix(matrix<T> * x) {

	pair<T, T> minMax = getMatrixMinMax<T>(x);
	if (minMax.first >= 0) // If content not have no negative values -> do nothing
	{
		return;
	}
	else // else, add make all values geater, than zero by substracting min value
	{
		for (int m = 0; m < (x)->size1(); ++m)
			for (int n = 0; n < (x)->size2(); n++)
			{
				T old_value = (x)->operator()(m, n);
				(x)->operator()(m, n) = old_value - minMax.first;
			}
	}
}


#pragma once
#include "trajopt/robot_and_dof.hpp"

#include "osgviewer/osgviewer.hpp"
#include <boost/random/normal_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>

#include "utils/eigen_conversions.hpp"

using namespace std;
using namespace Eigen;
using namespace util;

namespace trajopt {

#define STEP 0.00048828125

class TRAJOPT_API BeliefRobotAndDOF : public RobotAndDOF {
public:
	BeliefRobotAndDOF(OpenRAVE::RobotBasePtr _robot, const IntVec& _joint_inds, int _affinedofs=0, const OR::Vector _rotationaxis=OR::Vector());

	void ForwardKinematics(const VectorXd& dofs, Vector3d& eetrans);

	void SetBeliefValues(const DblVec& theta);
	DblVec GetBeliefValues();

	MatrixXd GetDynNoise();
	MatrixXd GetObsNoise();

	VectorXd Observe(const VectorXd& x, const VectorXd& r);
	VectorXd Dynamics(const VectorXd& x, const VectorXd& u, const VectorXd& q);

	MatrixXd dfdx(const VectorXd& x, const VectorXd& u, const VectorXd& q);
	MatrixXd dfdq(const VectorXd& x, const VectorXd& u, const VectorXd& q);
	MatrixXd dhdx(const VectorXd& x, const VectorXd& r);
	MatrixXd dhdr(const VectorXd& x, const VectorXd& r);

	VectorXd BeliefDynamics(const VectorXd& theta0, const VectorXd& u0);
	MatrixXd dgdb(const VectorXd& theta, const VectorXd& u);
	MatrixXd dgdu(const VectorXd& theta, const VectorXd& u);

	VectorXd VectorXdRand(int size);
	VectorXd mvnrnd(const VectorXd& mu, const MatrixXd& Sigma);

	template <typename T> Matrix<T,Dynamic,1> toSigmaVec(const Matrix<T,Dynamic,Dynamic>& rt_S) {
		Matrix<T,Dynamic,1> rt_S_vec(s_dim);
		int idx = 0;
		for (int i=0; i < x_dim; i++) {
			for (int j=i; j < x_dim; j++) {
				rt_S_vec[idx] = 0.5 * (rt_S(i,j)+rt_S(j,i));
				idx++;
			}
		}
		return rt_S_vec;
	}

	template <typename T> Matrix<T,Dynamic,Dynamic> toSigmaMatrix(const Matrix<T,Dynamic,1>& rt_S_vec) {
		Matrix<T,Dynamic,Dynamic> rt_S(x_dim, x_dim);
		int idx = 0;
		for (int j = 0; j < x_dim; ++j) {
			for (int i = j; i < x_dim; ++i) {
				rt_S(i,j) = rt_S(j,i) = rt_S_vec[idx];
				idx++;
			}
		}
		return rt_S;
	}

	void composeBelief(const VectorXd& x, const MatrixXd& rt_S, VectorXd& theta);
	void decomposeBelief(const VectorXd& theta, VectorXd& x, MatrixXd& rt_S);

	void ekfUpdate(const VectorXd& u0, const VectorXd& x0, const MatrixXd& rtSigma0, VectorXd& x, MatrixXd& rtSigma, bool observe, const VectorXd& z);

	MatrixXd sigmaPoints(const VectorXd& theta);
	MatrixXd sigmaPoints(const VectorXd& mean, const MatrixXd& sqrtcov);
	VectorXd sigmaPoint(const VectorXd& mean, const MatrixXd& cov, int idx);

	void ukfUpdate(const VectorXd& u0, const VectorXd& x0, const MatrixXd& rtSigma0, VectorXd& x, MatrixXd& rtSigma, bool observe, const VectorXd& z);

	void GetEndEffectorNoiseAsGaussian(const VectorXd& theta, Vector3d& mean, Matrix3d& cov);

	MatrixXd djdb(const VectorXd& theta, int sigma_pt_ind);
	// theta needs to be set accordingly before calling this (i.e. call SetBeliefValues)
	MatrixXd BeliefJacobian(int link_ind, int sigma_pt_ind, const OR::Vector& pt);

	void SetSigmaPointsScale(double scale) { sigma_pts_scale = scale; }

	inline int GetXDim() { return x_dim; }
	inline int GetBDim() { return b_dim; }
	inline int GetSDim() { return s_dim; }
	inline int GetQDim() { return q_dim; }
	inline int GetZDim() { return z_dim; }
	inline int GetRDim() { return r_dim; }
	inline int GetUDim() { return u_dim; }

	OR::KinBody::LinkPtr endeffector;
	// Scaled UKF update vars
	double alpha, beta, kappa;

private:
	double sigma_pts_scale;
	// returns the indices of the terms in Sigma_vec that are in the jth column of the corresponding Sigma_matrix
	inline vector<int> sigmaColToSigmaIndices(int j) { return sigma_col_to_indices[j]; }
	vector<vector<int> > sigma_col_to_indices;
	boost::variate_generator<boost::mt19937, boost::normal_distribution<> > generator;
	DblVec sigma_vec;
	int x_dim, b_dim, s_dim, z_dim, q_dim, r_dim, u_dim;
};

typedef boost::shared_ptr<BeliefRobotAndDOF> BeliefRobotAndDOFPtr;

osg::Matrix gaussianAsTransform(const Vector3d& mean, const Matrix3d& cov);

}


#include "utils/plotting.cpp"
#include "utils/data_generator.cpp"
#include "../src/analytical/linear/ls_solver.h"
#include "../src/numerical/gauss-newton/gn_solver.h"
#include "../src/numerical/gradient_descent/gd_solver.h"
#include "../src/robust/ransac_solver.cpp"
#include "../src/logging/easylogging++.h"
#include <armadillo>

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

INITIALIZE_EASYLOGGINGPP

arma::mat WEIGHTS = {100, -20, 50, -0.5};

int main(int argc, char *argv[])
{
    el::Configurations conf("./logging-config.conf");
    el::Loggers::reconfigureLogger("default", conf);

    auto data_generator = DataGenerator();
    auto L = data_generator.generate_library();
    std::vector<int> outlier_indices = {0, 5, 11, 13, 15, 18, 20, 25, 40, 60};
    auto s = data_generator.generate_signal(WEIGHTS, outlier_indices);

    LOG(INFO) << "True: " << WEIGHTS;

    auto solver = GDSolver(L);
    arma::mat result = solver.solve(s);
    LOG(INFO) << "Regular fit: " << result;

    int n_channels = 4;
    int n_max_iter = 1000;
    float accepted_error = 0.1;
    int n_accepted_points = 70;
    float objective_value_threshold = 0.0001;
    std::shared_ptr<GDSolver> gd_solver_ptr(new GDSolver(solver));
    auto ransac_solver = RansacSolver(gd_solver_ptr, n_channels, accepted_error, n_accepted_points, objective_value_threshold, n_max_iter);
    auto solution = ransac_solver.solve(s);
    LOG(INFO) << "RANSAC fit: " << solution;

    auto estimate = ransac_solver.get_signal_estimate();

#ifdef PLOT_FIGURES
    plot_arma_vec(s);
    plot_arma_vec(estimate);
    plt::show();
#endif

    return 0;
}


#include <iostream>
using std::cout; using std::endl;
using std::left; using std::fixed; using std::right; using std::scientific;
#include <iomanip>
using std::setw;
using std::setprecision;
#include <limits>

#include <algorithm>
#include <string>
#include <vector>

#include <boost/multiprecision/cpp_dec_float.hpp>
using boost::multiprecision::cpp_dec_float_50;

#include <boost/random.hpp>
#include <boost/random/exponential_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/lexical_cast.hpp>

#include <boost/math/distributions/binomial.hpp>
#include <boost/math/distributions/beta.hpp>
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/distributions/gamma.hpp>
#include <boost/math/distributions/geometric.hpp>
#include <boost/math/distributions/hypergeometric.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/poisson.hpp>
#include <boost/math/distributions/students_t.hpp>

int main(int argc, const char* argv[])
{
    std::string fun(argv[1]);

    int N = 200;
    if (argc >= 4)
    {
        N = boost::lexical_cast<unsigned long>(argv[2]);
    }

    unsigned long S = 17;
    if (argc >= 5)
    {
        S = boost::lexical_cast<unsigned long>(argv[3]);
        std::cerr << S << endl;
    }
    boost::random::mt19937 rng(S);
    boost::random::uniform_real_distribution<> runif;

    std::cout.precision(std::numeric_limits<cpp_dec_float_50>::digits10);

    if (fun == "beta")
    {
        boost::random::exponential_distribution<double> rexp(0.01);

        cout << "seed: " << S << endl;
        cout << "data:" << endl;
        for (int i = 0; i < N; ++i)
        {
            unsigned long a = 1 + static_cast<unsigned long>(rexp(rng));
            unsigned long b = 1 + static_cast<unsigned long>(rexp(rng));
            cpp_dec_float_50 x = runif(rng);
            boost::math::beta_distribution<cpp_dec_float_50> dst(a, b);
            cout << "- [" << a << ", " << b << ", " << x
                               << ", " << mean(dst)
                               << ", " << variance(dst)
                               << ", " << pdf(dst, x)
                               << ", " << cdf(dst, x)
                               << ", " << cdf(complement(dst, x))
                               << "]" << endl;
        }
        return 0;
    }

    if (fun == "binom")
    {
        boost::random::exponential_distribution<double> rexp(0.005);

        cout << "seed: " << S << endl;
        cout << "data:" << endl;
        for (int i = 0; i < N; ++i)
        {
            cpp_dec_float_50 p = runif(rng);
            unsigned long n = 1 + static_cast<unsigned long>(rexp(rng));
            unsigned long k = static_cast<unsigned long>((n+1)*runif(rng));
            boost::math::binomial_distribution<cpp_dec_float_50> dst(n, p);
            cout << "- [" << n << ", " << p << ", " << k
                               << ", " << mean(dst)
                               << ", " << variance(dst)
                               << ", " << pdf(dst, k)
                               << ", " << cdf(dst, k)
                               << ", " << cdf(complement(dst, k))
                               << "]" << endl;
        }
        return 0;
    }

    if (fun == "chisq")
    {
        boost::random::exponential_distribution<double> rexp1(0.05);
        boost::random::exponential_distribution<double> rexp2(0.01);

        cout << "seed: " << S << endl;
        cout << "data:" << endl;
        for (int i = 0; i < N; ++i)
        {
            unsigned long n = 1 + static_cast<unsigned long>(rexp1(rng));
            cpp_dec_float_50 x = rexp2(rng);
            boost::math::chi_squared_distribution<cpp_dec_float_50> dst(n);
            cout << "- [" << n << ", " << x
                               << ", " << mean(dst)
                               << ", " << variance(dst)
                               << ", " << pdf(dst, x)
                               << ", " << cdf(dst, x)
                               << ", " << cdf(complement(dst, x))
                               << "]" << endl;
        }
        return 0;
    }

    if (fun == "gamma")
    {
        boost::random::exponential_distribution<double> rexp1(0.05);
        boost::random::exponential_distribution<double> rexp2(0.01);

        cout << "seed: " << S << endl;
        cout << "data:" << endl;
        for (int i = 0; i < N; ++i)
        {
            cpp_dec_float_50 a = 1 + static_cast<unsigned long>(rexp1(rng));
            cpp_dec_float_50 x = rexp2(rng);
            boost::math::gamma_distribution<cpp_dec_float_50> dst(a);
            cout << "- [" << a << ", " << x
                               << ", " << mean(dst)
                               << ", " << variance(dst)
                               << ", " << pdf(dst, x)
                               << ", " << cdf(dst, x)
                               << ", " << cdf(complement(dst, x))
                               << "]" << endl;
        }
        return 0;
    }

    if (fun == "geom")
    {
        boost::random::exponential_distribution<double> rexp(0.02);

        cout << "seed: " << S << endl;
        cout << "data:" << endl;
        for (int i = 0; i < N; ++i)
        {
            cpp_dec_float_50 p = runif(rng);
            unsigned long k = static_cast<unsigned long>(rexp(rng));
            boost::math::geometric_distribution<cpp_dec_float_50> dst(p);
            cout << "- [" << p << ", " << k
                               << ", " << mean(dst)
                               << ", " << variance(dst)
                               << ", " << pdf(dst, k)
                               << ", " << cdf(dst, k)
                               << ", " << cdf(complement(dst, k))
                               << "]" << endl;
        }
        return 0;
    }

    if (fun == "hyper")
    {
        boost::random::exponential_distribution<double> rexp(0.02);

        cout << "seed: " << S << endl;
        cout << "data:" << endl;
        for (int i = 0; i < N; ++i)
        {
            unsigned long M = 1 + static_cast<unsigned long>(rexp(rng));
            unsigned long K = 1 + static_cast<unsigned long>(rexp(rng));
            unsigned long N = M + K;
            unsigned long n = static_cast<unsigned long>((N+1)*runif(rng));
            unsigned long k = static_cast<unsigned long>((n+1)*runif(rng));
            while (k > K || n - k > M) {
                k = static_cast<unsigned long>((n+1)*runif(rng));
            }
            boost::math::hypergeometric_distribution<cpp_dec_float_50> dst(K, n, N);
            cout << "- [" << N << ", " << K << ", " << n << ", " << k
                               << ", " << mean(dst)
                               << ", " << variance(dst)
                               << ", " << pdf(dst, k)
                               << ", " << cdf(dst, k)
                               << ", " << cdf(complement(dst, k))
                               << "]" << endl;
        }
        return 0;
    }

    if (fun == "norm")
    {
        boost::random::exponential_distribution<double> rexp(0.02);
        boost::random::exponential_distribution<double> rexp2(0.005);

        cout << "seed: " << S << endl;
        cout << "data:" << endl;
        for (int i = 0; i < N; ++i)
        {
            cpp_dec_float_50 mu = rexp(rng);
            if (runif(rng) < 0.5) {
                mu = -mu;
            }
            cpp_dec_float_50 sig = rexp(rng);
            cpp_dec_float_50 x = rexp2(rng);
            if (runif(rng) < 0.5) {
                x = -x;
            }
            boost::math::normal_distribution<cpp_dec_float_50> dst(mu, sig);
            cout << "- [" << mu << ", " << sig << ", " << x
                               << ", " << mean(dst)
                               << ", " << variance(dst)
                               << ", " << pdf(dst, x)
                               << ", " << cdf(dst, x)
                               << ", " << cdf(complement(dst, x))
                               << "]" << endl;
        }
        return 0;
    }

    if (fun == "pois")
    {
        boost::random::exponential_distribution<double> rexp(0.02);
        boost::random::exponential_distribution<double> rexp2(0.005);

        cout << "seed: " << S << endl;
        cout << "data:" << endl;
        for (int i = 0; i < N; ++i)
        {
            cpp_dec_float_50 lam = rexp(rng);
            unsigned long k = static_cast<unsigned int>(rexp(rng));
            if (runif(rng) < 0.5)
            {
                k = static_cast<unsigned int>(rexp2(rng));
            }
            boost::math::poisson_distribution<cpp_dec_float_50> dst(lam);
            cout << "- [" << lam << ", " << k
                               << ", " << mean(dst)
                               << ", " << variance(dst)
                               << ", " << pdf(dst, k)
                               << ", " << cdf(dst, k)
                               << ", " << cdf(complement(dst, k))
                               << "]" << endl;
        }
        return 0;
    }

    if (fun == "stud")
    {
        boost::random::exponential_distribution<double> rexp(0.075);
        boost::random::exponential_distribution<double> rexp2(0.005);

        cout << "seed: " << S << endl;
        cout << "data:" << endl;
        for (int i = 0; i < N; ++i)
        {
            cpp_dec_float_50 nu = 2 + rexp(rng);
            cpp_dec_float_50 x = static_cast<unsigned int>(rexp2(rng));
            if (runif(rng) < 0.5)
            {
                x = -x;
            }
            boost::math::students_t_distribution<cpp_dec_float_50> dst(nu);
            cout << "- [" << nu << ", " << x
                               << ", " << mean(dst)
                               << ", " << variance(dst)
                               << ", " << pdf(dst, x)
                               << ", " << cdf(dst, x)
                               << ", " << cdf(complement(dst, x))
                               << "]" << endl;
        }
        return 0;
    }

   return 1;
}


/************************************************************
 *
 * Copyright (c) 2021, University of California, Los Angeles
 *
 * Authors: Kenny J. Chen, Brett T. Lopez
 * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu
 *
 ***********************************************************/

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

#ifndef NANO_GICP_SO3_HPP
#define NANO_GICP_SO3_HPP

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

namespace nano_gicp {

inline Eigen::Matrix3f skew(const Eigen::Vector3f& x) {
  Eigen::Matrix3f skew = Eigen::Matrix3f::Zero();
  skew(0, 1) = -x[2];
  skew(0, 2) = x[1];
  skew(1, 0) = x[2];
  skew(1, 2) = -x[0];
  skew(2, 0) = -x[1];
  skew(2, 1) = x[0];

  return skew;
}

inline Eigen::Matrix3d skewd(const Eigen::Vector3d& x) {
  Eigen::Matrix3d skew = Eigen::Matrix3d::Zero();
  skew(0, 1) = -x[2];
  skew(0, 2) = x[1];
  skew(1, 0) = x[2];
  skew(1, 2) = -x[0];
  skew(2, 0) = -x[1];
  skew(2, 1) = x[0];

  return skew;
}

/*
 * SO3 expmap code taken from Sophus
 * https://github.com/strasdat/Sophus/blob/593db47500ea1a2de5f0e6579c86147991509c59/sophus/so3.hpp#L585
 *
 * Copyright 2011-2017 Hauke Strasdat
 *           2012-2017 Steven Lovegrove
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal in the Software without restriction, including without limitation the
 * rights  to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * IN THE SOFTWARE.
 */
inline Eigen::Quaterniond so3_exp(const Eigen::Vector3d& omega) {
  double theta_sq = omega.dot(omega);

  double theta;
  double imag_factor;
  double real_factor;
  if(theta_sq < 1e-10) {
    theta = 0;
    double theta_quad = theta_sq * theta_sq;
    imag_factor = 0.5 - 1.0 / 48.0 * theta_sq + 1.0 / 3840.0 * theta_quad;
    real_factor = 1.0 - 1.0 / 8.0 * theta_sq + 1.0 / 384.0 * theta_quad;
  } else {
    theta = std::sqrt(theta_sq);
    double half_theta = 0.5 * theta;
    imag_factor = std::sin(half_theta) / theta;
    real_factor = std::cos(half_theta);
  }

  return Eigen::Quaterniond(real_factor, imag_factor * omega.x(), imag_factor * omega.y(), imag_factor * omega.z());
}

}  // namespace nano_gicp

#endif


#include "initial_conditions/radial_wave.hpp"
#include <cmath>
#include <boost/test/unit_test.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/sum.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <fstream>
#include <iostream>

using namespace boost::accumulators;

using accumulator_t = accumulator_set<double, stats<tag::sum, tag::mean, tag::variance, tag::max, tag::min > >;

BOOST_AUTO_TEST_SUITE(radial_init_tests)

BOOST_AUTO_TEST_CASE(phi_dist)
{
    using namespace boost;

    init_cond::RadialWave3D wave(3);
    wave.setEnergyNormalization(false);
    wave.setParticleCount(1000000);
    wave.init();

    std::size_t num_bins = 180;

    std::vector<std::size_t> counts(num_bins, 0);
    // fix range
    for(InitialCondition cond = wave.next(); cond; ++cond)
    {
        auto& vel = cond.getState().getVelocity();
        double x = vel[0];
        double y = vel[1];
        double z = vel[2];
        double c = std::sqrt(1 - z*z);
        double phi = std::atan2(y/c, x/c);
        //double azi = std::asin( z ) * 90 / std::acos(0);
        double phi_deg = std::floor(phi * 90 / std::acos(0) + 180);
        ++counts.at( int(phi_deg * num_bins / 360) );
    }

    std::fstream res("result.txt", std::fstream::out);

    accumulator_t acc;

    for(int i = 0; i < num_bins; ++i)
    {
        res << i << " " << counts[i] << std::endl;
        acc(counts[i]);
    }

    double mx = max(acc);
    double mi = min(acc);
    double disx = (mx / mean(acc)-1) * 100;
    double disi = (1 - mi / mean(acc)) * 100;
    double max_relative_error = std::max(disx, disi);

    double mean_relative_error = std::sqrt(variance(acc) / sum(acc)) / num_bins * 100;

    // check that mean error is lower than .025%
    BOOST_CHECK( mean_relative_error < 0.025 );

    // check that maximum error is lower than 4.2%
    BOOST_CHECK( max_relative_error < 4.2 );

    std::cout << "mean relative error: " << mean_relative_error << "%\n";
    std::cout << "max relative error: "  << max_relative_error << "%\n";
}

BOOST_AUTO_TEST_SUITE_END()


#include <dlib/dnn.h>
#include <dlib/matrix.h>

#include <experimental/filesystem>
#include <iostream>
#include <random>
#include <regex>
#include <sstream>
namespace fs = std::experimental::filesystem;

int main(int argc, char** argv) {
  using namespace dlib;
  if (argc > 1) {
    if (fs::exists(argv[1])) {
      std::stringstream str_stream;
      {
        // replace categorial values with numeric ones
        std::ifstream data_stream(argv[1]);
        std::string data_string((std::istreambuf_iterator<char>(data_stream)),
                                std::istreambuf_iterator<char>());

        // replace string labels, because SharkML parssr can't handle strings
        data_string =
            std::regex_replace(data_string, std::regex("Iris-setosa"), "1");
        data_string =
            std::regex_replace(data_string, std::regex("Iris-versicolor"), "2");
        data_string =
            std::regex_replace(data_string, std::regex("Iris-virginica"), "3");

        str_stream << data_string;
      }

      matrix<double> data;
      str_stream >> data;

      std::cout << data << std::endl;

      matrix<double> x_data = subm(data, 0, 0, data.nr(), data.nc() - 1);

      std::vector<matrix<double>> samples;
      for (int r = 0; r < x_data.nr(); ++r) {
        samples.push_back(rowm(x_data, r));
      }

      // Normalization
      // Standardization
      matrix<double> m(mean(mat(samples)));
      matrix<double> sd(reciprocal(stddev(mat(samples))));
      for (size_t i = 0; i < samples.size(); ++i)
        samples[i] = pointwise_multiply(samples[i] - m, sd);
      std::cout << mat(samples) << std::endl;

      // Another approach
      // vector_normalizer<matrix<double>> normalizer;
      // samples normalizer.train(samples);
      // samples = normalizer(samples);
    } else {
      std::cerr << "Invalid file path " << argv[1] << std::endl;
    }
  } else {
    std::cerr << "Please provide a path to a dataset\n";
  }
  return 0;
}


#include <iostream>

#include <harp_test.hpp>

#include <boost/random.hpp>

using namespace std;
using namespace harp;

#define DATASIZE 100
#define SIGSIZE 6
#define MAX 1000.0
#define TOL 1.0e-6

void harp::test_invcov ( string const & datadir ) {

  int np;
  int myp;

  MPI_Comm_size ( MPI_COMM_WORLD, &np );
  MPI_Comm_rank ( MPI_COMM_WORLD, &myp );

  El::Grid grid ( El::mpi::COMM_WORLD );

  if ( myp == 0 ) {
    cerr << "Testing inverse covariance construction..." << endl;
  }

  cerr.precision(15);
  cout.precision(15);

  // construct random sparse matrix

  matrix_sparse AT ( SIGSIZE, DATASIZE, El::mpi::COMM_WORLD );

  matrix_local compAT ( SIGSIZE, DATASIZE );
  local_matrix_zero ( compAT );

  size_t local_firstrow = AT.FirstLocalRow();
  size_t local_rows = AT.LocalHeight();

  typedef boost::ecuyer1988 base_generator_type;
  base_generator_type generator(42u);

  double rms = 0.5;
  boost::normal_distribution < double > dist ( 0.0, rms );
  boost::variate_generator < base_generator_type&, boost::normal_distribution < double > > gauss ( generator, dist );

  AT.StartAssembly();

  size_t nnz = 10;

  AT.Reserve ( nnz * local_rows );

  size_t col;

  for ( size_t i = 0; i < SIGSIZE; ++i ) {

    for ( size_t j = 0; j < nnz; ++j ) {

      col = (size_t)( 2 * i + 8*j );

      compAT.Set ( i, col, 1.0 );

      if ( ( i >= local_firstrow ) && ( i < local_firstrow + local_rows ) ) {
        AT.Update ( i, col, 1.0 );
      }

    }

  }

  AT.StopAssembly();

  // truth

  matrix_dist truth ( SIGSIZE, 1, grid );

  matrix_local signal ( DATASIZE, 1 );

  for ( int i = 0; i < SIGSIZE; ++i ) {
    truth.Set ( i, 0, 40.0 + 10.0*(double)i );
  }

  spec_project ( AT, truth, signal );

  // fake noise covariance and measured data

  matrix_local noise ( DATASIZE, 1 );

  matrix_local measured ( DATASIZE, 1 );

  matrix_local invpix ( DATASIZE, 1 );

  for ( size_t i = 0; i < DATASIZE; ++i ) {
    invpix.Set ( i, 0, 1.0/(rms * rms) );
    noise.Set ( i, 0, gauss() );
    measured.Set ( i, 0, signal.Get(i,0) + noise.Get(i,0) );
  }

  // RHS

  matrix_dist z ( SIGSIZE, 1, grid );

  noise_weighted_spec ( AT, invpix, measured, z );

  // construct test output

  matrix_local compinv ( SIGSIZE, SIGSIZE );
  local_matrix_zero ( compinv );

  matrix_local compATN ( compAT );

  for ( size_t i = 0; i < SIGSIZE; ++i ) {

    for ( size_t j = 0; j < nnz; ++j ) {

      col = (size_t)( 2 * i + 8*j );

      compATN.Set ( i, col, 1.0/(rms * rms) );

    }

  }

  El::Gemm ( El::NORMAL, El::TRANSPOSE, 1.0, compATN, compAT, 0.0, compinv );

  if ( myp == 0 ) {
    El::Print ( compinv, "Serial inverse covariance" );
  }

  // parallel implementation

  matrix_dist inv ( SIGSIZE, SIGSIZE, grid );

  inverse_covariance ( AT, invpix, inv );

  El::Print ( inv, "MPI inverse covariance" );

  // compare results in lower triangle

  matrix_local local_inv ( SIGSIZE, SIGSIZE );
  local_matrix_zero ( local_inv );

  El::AxpyInterface < double > globloc;
  globloc.Attach( El::GLOBAL_TO_LOCAL, inv );
  globloc.Axpy ( 1.0, local_inv, 0, 0 );
  globloc.Detach();

  for ( size_t i = 0; i < SIGSIZE; ++i ) {
    for ( size_t j = i; j < SIGSIZE; ++j ) {
      double locval = local_inv.Get ( j, i );
      double compval = compinv.Get ( j, i );
      if ( fabs ( locval - compval ) / compval > TOL ) {
        cerr << "proc " << myp << " FAIL on element (" << j << ", " << i << "); " << locval << " != " << compval << endl;
        exit(1);
      }

    }
  }

  if ( myp == 0 ) {
    cerr << "  (PASSED)" << endl;
    cerr << "Testing extraction..." << endl;
  }

  matrix_dist W ( SIGSIZE, SIGSIZE, grid );
  matrix_dist D ( SIGSIZE, 1, grid );
  eigen_decompose ( inv, D, W );

  matrix_dist sq ( SIGSIZE, SIGSIZE, grid );
  eigen_compose ( EIG_SQRT, D, W, sq );

  El::Print ( sq, "sqrt of invcov" );

  matrix_dist Rdirect ( sq );
  matrix_dist R ( SIGSIZE, SIGSIZE, grid );
  matrix_dist S ( SIGSIZE, 1, grid );

  norm ( D, W, S );

  El::Print ( S, "column norm from eigen decomposition" );

  apply_norm ( S, Rdirect );

  El::Print ( Rdirect, "direct resolution matrix" );

  resolution ( D, W, S, R );

  El::Print ( R, "resolution matrix" );

  matrix_dist f ( SIGSIZE, 1, grid );

  matrix_dist Rf ( SIGSIZE, 1, grid );

  extract ( D, W, S, z, Rf, f );

  matrix_dist Rtruth ( SIGSIZE, 1, grid );

  El::Gemv ( El::NORMAL, 1.0, R, truth, 0.0, Rtruth );

  for ( size_t i = 0; i < SIGSIZE; ++i ) {
    double tr = truth.Get ( i, 0 );
    double rt = Rtruth.Get ( i, 0 );
    double rf = Rf.Get ( i, 0 );
    double errval = S.Get ( i, 0 );
    double ze = z.Get ( i, 0 );
    if ( myp == 0 ) {
      cout << "  truth = " << tr << ", z = " << ze << ", convolved = " << rt << ", Rf = " << rf << ", err = " << errval << endl;
    }
  }

  if ( myp == 0 ) {
    cerr << "  (PASSED)" << endl;
  }

  return;
}


#include <iostream>
#include <boost/numeric/interval.hpp>

int main()
{
	boost::numeric::interval<int> range1(0, 100);
	boost::numeric::interval<int> range2(30, 120);

	boost::numeric::interval<int> new_range1 = range1 + range2;

	std::cout << new_range1.lower() << " ~ "
		<< new_range1.upper() << std::endl;

	boost::numeric::interval<int> range3(10, 400);
	range3 += range2;

	std::cout << range3.lower() << " ~ "
		<< range3.upper() << std::endl;

	boost::numeric::interval<int> range4(0, 100);
	range4 += 1;

	std::cout << range4.lower() << " ~ "
		<< range4.upper() << std::endl;

	return 0;
}


#include <codegenvar/Eigen>
#include <codegenvar/BooleanEvaluator.h>
#include <Eigen/Dense>
#include <iostream>

using namespace codegenvar;

int main()
{
    Mat2 m2 = namedMatrix("m", 2, 2);
    std::cout << "m' = " << std::endl << m2.inverse() << std::endl << std::endl;

    BooleanEvaluator evaluator;
    Mat m = namedMatrix("m", 2, 2);
    Mat inv(2, 2);
    do
    {
        inv |= m.inverse();
    } while (!evaluator.isFullyEvaluated());

    std::cout << "m11' = " << std::endl << inv(0,0) << std::endl;
    return 0;
}


#include <limits>
#include <iostream>
#include <iomanip>
#include <string>
#include <cmath>
#include <algorithm>
#include <vector>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <CGAL/Triangulation_face_base_2.h>
#include <boost/pending/disjoint_sets.hpp>

// Epic kernel is enough, no constructions needed, provided the squared distance
// fits into a double (!)
typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
// we want to store an index with each vertex
typedef std::size_t                                            Index;
typedef CGAL::Triangulation_vertex_base_with_info_2<Index,K>   Vb;
typedef CGAL::Triangulation_face_base_2<K>                     Fb;
typedef CGAL::Triangulation_data_structure_2<Vb,Fb>            Tds;
typedef CGAL::Delaunay_triangulation_2<K,Tds>                  Delaunay;

typedef std::tuple<Index,Index,K::FT> Edge;
typedef std::vector<Edge> EdgeV;

int max_num_fam(std::vector<int> &comp_of_size, int k) {
    // vector is size == k+1
    int num = comp_of_size[k];
    if(k == 4) {
        // match 3 with ones, then take pairs of 2, match rest
        int match_three_one = std::min(comp_of_size[3], comp_of_size[1]);
        int remaining3 = comp_of_size[3] - match_three_one;
        int remaining1 = comp_of_size[1] - match_three_one;
        // add remaining size 3 to 2
        int remaining2 = comp_of_size[2] + remaining3;
        // if num2 is not divisible by two, we can possibly combine
        // the one leftover with the single
        if(remaining2 % 2 == 1) remaining1 += 2;
        num += match_three_one + remaining2 / 2 + remaining1 / 4;

    } else if(k == 3) {
        // match 2 with ones, add rest
        int match_two_one = std::min(comp_of_size[2], comp_of_size[1]);
        int remaining2 = comp_of_size[2] - match_two_one;
        int remaining1 = comp_of_size[1] - match_two_one;
        // the remaining 2size comp have to be div by 2 (two together is one family)
        num += match_two_one + remaining2 / 2 + remaining1 / 3;
    } else if(k == 2) {
        // just take the single comp and divide by 2
        num += comp_of_size[1] / 2;
    }
    return num;
}

void testcase() {
    Index n, k, f0;
    double s0;
    std::cin >> n >> k >> f0 >> s0;

    typedef std::pair<K::Point_2,Index> IPoint;
    std::vector<IPoint> points;
    points.reserve(n);
    for (Index i = 0; i < n; ++i) {
        int x, y;
        std::cin >> x >> y;
        points.emplace_back(K::Point_2(x, y), i);
    }
    Delaunay t;
    t.insert(points.begin(), points.end());
    EdgeV edges;
    edges.reserve(3*n); // there can be no more in a planar graph
    for (auto e = t.finite_edges_begin(); e != t.finite_edges_end(); ++e) {
        Index i1 = e->first->vertex((e->second+1)%3)->info();
        Index i2 = e->first->vertex((e->second+2)%3)->info();
        // ensure smaller index comes first
        if (i1 > i2) std::swap(i1, i2);
        edges.emplace_back(i1, i2, t.segment(e).squared_length());
    }
    std::sort(edges.begin(), edges.end(),
            [](const Edge& e1, const Edge& e2) -> bool {
            return std::get<2>(e1) < std::get<2>(e2);
                });

    // for testcases 1-2: just look at smallest distance, bc otherwise not enough tents
    // std::cout << long(std::get<2>(edges[0])) << " ";

    boost::disjoint_sets_with_storage<> uf(n);
    std::vector<Index> num_tents(n, 1);
    Index n_components = n;
    std::vector<int> comp_of_size(k + 1, 0);
    comp_of_size[1] = n;
    double last_dist = 0;
    for (EdgeV::const_iterator e = edges.begin(); e != edges.end(); ++e) {
        // determine components of endpoints
        Index c1 = uf.find_set(std::get<0>(*e));
        Index c2 = uf.find_set(std::get<1>(*e));
        last_dist = std::get<2>(*e);
        if (c1 != c2) {
            Index n1 = num_tents[c1];
            Index n2 = num_tents[c2];
            uf.link(c1, c2);
            Index c3 = uf.find_set(std::get<1>(*e));
            // set unused indices to zero
            num_tents[c1] = num_tents[c2] = 0;
            // cap it at k
            num_tents[c3] = std::min(n1 + n2, k);
            comp_of_size[n1]--; comp_of_size[n2]--;
            comp_of_size[num_tents[c3]]++;
            if (max_num_fam(comp_of_size, k) < f0) break;
        }
    }

    std::cout << long(last_dist) << " ";

    // repeat process with adding edges < s0, then find max num families
    boost::disjoint_sets_with_storage<> uf_s0(n);
    num_tents = std::vector<Index>(n, 1);
    comp_of_size = std::vector<int>(k + 1, 0);
    comp_of_size[1] = n;

    n_components = n;
    for (EdgeV::const_iterator e = edges.begin(); e != edges.end(); ++e) {
        // determine components of endpoints
        Index c1 = uf_s0.find_set(std::get<0>(*e));
        Index c2 = uf_s0.find_set(std::get<1>(*e));
        double dist = std::get<2>(*e);
        if(dist >= s0) {
            break;
        }
        if (c1 != c2) {
            Index n1 = num_tents[c1];
            Index n2 = num_tents[c2];
            uf_s0.link(c1, c2);
            Index c3 = uf_s0.find_set(std::get<1>(*e));
            // set unused indices to zero
            num_tents[c1] = num_tents[c2] = 0;
            // cap component size at k
            num_tents[c3] = std::min(n1 + n2, k);
            comp_of_size[n1]--; comp_of_size[n2]--;
            comp_of_size[num_tents[c3]]++;
            if (--n_components == 1) break;
        }
    }
    std::cout << max_num_fam(comp_of_size, k) << std::endl;
    return;
}

int main() {
    std::ios_base::sync_with_stdio(false);

    int t;
    std::cin >> t;
    for (int i = 0; i < t; ++i)
        testcase();
}


#include <iostream>
#include <cassert>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS> GraphTraits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
                              boost::property<boost::edge_capacity_t, int,
                                              boost::property<boost::edge_residual_capacity_t, int,
                                                              boost::property<boost::edge_reverse_t, GraphTraits::edge_descriptor>>>>
    Graph;

void testcase()
{
  int m, n, k, c;
  std::cin >> m >> n >> k >> c;
  assert(m >= 0 && m <= 50 && n >= 0 && n <= 50);
  assert(k >= 0 && k <= m * n);
  assert(c >= 0 && c <= 4);

  int total_nodes = 2 + 2 * m * n + m * (n + 1) + n * (m + 1);
  Graph G(total_nodes);
  int next_free_node = 0;
  Graph::vertex_descriptor source_node = next_free_node++;
  Graph::vertex_descriptor sink_node = next_free_node++;
  auto node_from_intersection = [next_free_node, m, n](int col, int row, bool source_part) -> Graph::vertex_descriptor {
    assert(col >= 0 && col < m && row >= 0 && row < n);
    return next_free_node + 2 * (col * n + row) + (source_part ? 1 : 0);
  };
  next_free_node += 2 * m * n;
  auto node_from_col_hall = [next_free_node, m, n](int col, int hall) -> Graph::vertex_descriptor {
    assert(col >= 0 && col < m && hall >= 0 && hall <= n);
    return next_free_node + col * (n + 1) + hall;
  };
  next_free_node += m * (n + 1);
  auto node_from_row_hall = [next_free_node, m, n](int row, int hall) -> Graph::vertex_descriptor {
    assert(row >= 0 && row < n && hall >= 0 && hall <= m);
    return next_free_node + row * (m + 1) + hall;
  };
  next_free_node += n * (m + 1);
  assert(next_free_node == total_nodes);

  auto capacity_map = boost::get(boost::edge_capacity, G);
  auto reverse_map = boost::get(boost::edge_reverse, G);
  auto add_edge = [&G, &capacity_map, &reverse_map, total_nodes](int from, int to, int capacity) {
    assert(from >= 0 && from < total_nodes && to >= 0 && to < total_nodes);
    Graph::edge_descriptor e = boost::add_edge(from, to, G).first;
    Graph::edge_descriptor rev_e = boost::add_edge(to, from, G).first;
    capacity_map[e] = capacity;
    capacity_map[rev_e] = 0;
    reverse_map[e] = rev_e;
    reverse_map[rev_e] = e;
  };

  for (int col = 0; col < m; col++)
  {
    add_edge(node_from_col_hall(col, 0), sink_node, 1);
    add_edge(node_from_col_hall(col, n), sink_node, 1);
    for (int i = 0; i < n; i++)
    {
      auto intersection_source = node_from_intersection(col, i, true);
      auto intersection_target = node_from_intersection(col, i, false);
      for (int hall : {i, i + 1})
      {
        auto hall_node = node_from_col_hall(col, hall);
        add_edge(hall_node, intersection_source, 1);
        add_edge(intersection_target, hall_node, 1);
      }
    }
  }

  for (int row = 0; row < n; row++)
  {
    add_edge(node_from_row_hall(row, 0), sink_node, 1);
    add_edge(node_from_row_hall(row, m), sink_node, 1);
    for (int i = 0; i < m; i++)
    {
      auto intersection_source = node_from_intersection(i, row, true);
      auto intersection_target = node_from_intersection(i, row, false);
      for (int hall : {i, i + 1})
      {
        auto hall_node = node_from_row_hall(row, hall);
        add_edge(hall_node, intersection_source, 1);
        add_edge(intersection_target, hall_node, 1);
      }
    }
  }

  for (int col = 0; col < m; col++)
  {
    for (int row = 0; row < n; row++)
    {
      add_edge(node_from_intersection(col, row, true), node_from_intersection(col, row, false), c);
    }
  }

  for (int i = 0; i < k; i++)
  {
    int x, y;
    std::cin >> x >> y;
    assert(x >= 0 && x < m && y >= 0 && y < n);
    add_edge(source_node, node_from_intersection(x, y, true), 1);
  }

  int flow = boost::push_relabel_max_flow(G, source_node, sink_node);
  std::cout << flow << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
  }

  return 0;
}