#define CATCH_CONFIG_MAIN

#include "CALPHADFreeEnergyFunctionsBinary.h"
#include "InterpolationType.h"

#include "catch.hpp"

#include <boost/optional/optional.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <string>

namespace pt = boost::property_tree;

TEST_CASE("CALPHAD binary equilibrium", "[binary equilibrium]")
{
    Thermo4PFM::EnergyInterpolationType energy_interp_func_type
        = Thermo4PFM::EnergyInterpolationType::PBG;
    Thermo4PFM::ConcInterpolationType conc_interp_func_type
        = Thermo4PFM::ConcInterpolationType::PBG;

    double temperature = 1423.;

    std::cout << " Read CALPHAD database..." << std::endl;
    pt::ptree calphad_db;
    try
    {
        pt::read_json("../thermodynamic_data/calphadAuNi.json", calphad_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }

    boost::optional<pt::ptree&> newton_db;

    Thermo4PFM::CALPHADFreeEnergyFunctionsBinary cafe(
        calphad_db, newton_db, energy_interp_func_type, conc_interp_func_type);

    // initial guesses
    double init_guess[2] = { 0.2, 0.1 };

    double lceq[2] = { init_guess[0], init_guess[1] };

    // compute equilibrium concentrations in each phase
    bool found_ceq = cafe.computeCeqT(temperature, &lceq[0]);
    if (lceq[0] > 1.) found_ceq = false;
    if (lceq[0] < 0.) found_ceq = false;
    if (lceq[1] > 1.) found_ceq = false;
    if (lceq[1] < 0.) found_ceq = false;

    std::cout << "Temperature = " << temperature << std::endl;
    if (found_ceq)
    {
        std::cout << "Found equilibrium concentrations: " << lceq[0] << " and "
                  << lceq[1] << "..." << std::endl;
    }
    REQUIRE(found_ceq);

    double expected_result[2] = { 0.283171, 0.108235 };

    CHECK(lceq[0] == Approx(expected_result[0]).margin(1.e-6));

    CHECK(lceq[1] == Approx(expected_result[1]).margin(1.e-6));
}


/****************************************************************************
**
** Copyright (C) 2017 TU Wien, ACIN, Vision 4 Robotics (V4R) group
** Contact: v4r.acin.tuwien.ac.at
**
** This file is part of V4R
**
** V4R is distributed under dual licenses - GPLv3 or closed source.
**
** GNU General Public License Usage
** V4R is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published
** by the Free Software Foundation, either version 3 of the License, or
** (at your option) any later version.
**
** V4R is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** Please review the following information to ensure the GNU General Public
** License requirements will be met: https://www.gnu.org/licenses/gpl-3.0.html.
**
**
** Commercial License Usage
** If GPL is not suitable for your project, you must purchase a commercial
** license to use V4R. Licensees holding valid commercial V4R licenses may
** use this file in accordance with the commercial license agreement
** provided with the Software or, alternatively, in accordance with the
** terms contained in a written agreement between you and TU Wien, ACIN, V4R.
** For licensing terms and conditions please contact office<at>acin.tuwien.ac.at.
**
**
** The copyright holder additionally grants the author(s) of the file the right
** to use, copy, modify, merge, publish, distribute, sublicense, and/or
** sell copies of their contributions without any restrictions.
**
****************************************************************************/

#include "v4r/attention_segmentation/PCA.h"
#include <Eigen/Dense>

namespace v4r {

Eigen::Vector4f getMean(pcl::PointCloud<pcl::Normal>::ConstPtr cloud) {
  Eigen::Vector4f mean;
  mean.setZero();

  for (unsigned int pi = 0; pi < cloud->size(); pi++) {
    mean[0] += cloud->points.at(pi).normal[0];
    mean[1] += cloud->points.at(pi).normal[1];
    mean[2] += cloud->points.at(pi).normal[2];
  }

  mean /= (float)cloud->size();

  return (mean);
}

bool computeCovarianceMatrix(pcl::PointCloud<pcl::Normal>::ConstPtr cloud, const Eigen::Vector4f &mean,
                             Eigen::Matrix3f &cov) {
  bool done = false;
  cov.setZero();

  for (unsigned pi = 0; pi < cloud->size(); ++pi) {
    float x = cloud->points.at(pi).normal[0] - mean[0];
    float y = cloud->points.at(pi).normal[1] - mean[1];
    float z = cloud->points.at(pi).normal[2] - mean[2];

    cov(0, 0) += x * x;
    cov(0, 1) += x * y;
    cov(0, 2) += x * z;

    cov(1, 0) += y * x;
    cov(1, 1) += y * y;
    cov(1, 2) += y * z;

    cov(2, 0) += z * x;
    cov(2, 1) += z * y;
    cov(2, 2) += z * z;

    done = true;
  }

  return (done);
}

void principleAxis(pcl::PointCloud<pcl::Normal>::ConstPtr cloud, std::vector<pcl::Normal> &axis) {
  Eigen::Vector4f mean;
  EIGEN_ALIGN16 Eigen::Matrix3f cov;
  EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
  EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;

  axis.clear();
  axis.resize(3);

  mean = getMean(cloud);

  if (computeCovarianceMatrix(cloud, mean, cov)) {
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver(cov);
    if (eigensolver.info() != Eigen::Success)
      return;

    eigen_values = eigensolver.eigenvalues();
    eigen_vectors = eigensolver.eigenvectors();

    axis.at(0).normal[0] = eigen_vectors(0, 0);
    axis.at(0).normal[1] = eigen_vectors(1, 0);
    axis.at(0).normal[2] = eigen_vectors(2, 0);

    axis.at(1).normal[0] = eigen_vectors(0, 1);
    axis.at(1).normal[1] = eigen_vectors(1, 1);
    axis.at(1).normal[2] = eigen_vectors(2, 1);

    axis.at(2).normal[0] = eigen_vectors(0, 2);
    axis.at(2).normal[1] = eigen_vectors(1, 2);
    axis.at(2).normal[2] = eigen_vectors(2, 2);
  }
}

}  // namespace v4r


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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)

#ifndef BOOST_UNITS_MEASUREMENT_HPP
#define BOOST_UNITS_MEASUREMENT_HPP

#include <cmath>
#include <cstdlib>
#include <iomanip>
#include <iostream>

#include <boost/io/ios_state.hpp>
#include <boost/units/static_rational.hpp>

namespace boost {

namespace units {

namespace sqr_namespace /**/ {

template<class Y>
Y sqr(Y val)
{ return val*val; }

} // namespace

using sqr_namespace::sqr;

template<class Y>
class measurement
{
    public:
        typedef measurement<Y>                  this_type;
        typedef Y                               value_type;

        measurement(const value_type& val = value_type(),
                    const value_type& err = value_type()) :
            value_(val),
            uncertainty_(std::abs(err))
        { }

        measurement(const this_type& source) :
            value_(source.value_),
            uncertainty_(source.uncertainty_)
        { }

        //~measurement() { }

        this_type& operator=(const this_type& source)
        {
            if (this == &source) return *this;

            value_ = source.value_;
            uncertainty_ = source.uncertainty_;

            return *this;
        }

        operator value_type() const    { return value_; }

        value_type value() const       { return value_; }
        value_type uncertainty() const { return uncertainty_; }
        value_type lower_bound() const { return value_-uncertainty_; }
        value_type upper_bound() const { return value_+uncertainty_; }

        this_type& operator+=(const value_type& val)
        {
            value_ += val;
            return *this;
        }

        this_type& operator-=(const value_type& val)
        {
            value_ -= val;
            return *this;
        }

        this_type& operator*=(const value_type& val)
        {
            value_ *= val;
            uncertainty_ *= val;
            return *this;
        }

        this_type& operator/=(const value_type& val)
        {
            value_ /= val;
            uncertainty_ /= val;
            return *this;
        }

        this_type& operator+=(const this_type& /*source*/);
        this_type& operator-=(const this_type& /*source*/);
        this_type& operator*=(const this_type& /*source*/);
        this_type& operator/=(const this_type& /*source*/);

    private:
        value_type          value_,
                            uncertainty_;
};

}

}

#if BOOST_UNITS_HAS_BOOST_TYPEOF

BOOST_TYPEOF_REGISTER_TEMPLATE(boost::units::measurement, 1)

#endif

namespace boost {

namespace units {

template<class Y>
inline
measurement<Y>&
measurement<Y>::operator+=(const this_type& source)
{
    uncertainty_ = std::sqrt(sqr(uncertainty_)+sqr(source.uncertainty_));
    value_ += source.value_;

    return *this;
}

template<class Y>
inline
measurement<Y>&
measurement<Y>::operator-=(const this_type& source)
{
    uncertainty_ = std::sqrt(sqr(uncertainty_)+sqr(source.uncertainty_));
    value_ -= source.value_;

    return *this;
}

template<class Y>
inline
measurement<Y>&
measurement<Y>::operator*=(const this_type& source)
{
    uncertainty_ = (value_*source.value_)*
              std::sqrt(sqr(uncertainty_/value_)+
                        sqr(source.uncertainty_/source.value_));
    value_ *= source.value_;

    return *this;
}

template<class Y>
inline
measurement<Y>&
measurement<Y>::operator/=(const this_type& source)
{
    uncertainty_ = (value_/source.value_)*
              std::sqrt(sqr(uncertainty_/value_)+
                        sqr(source.uncertainty_/source.value_));
    value_ /= source.value_;

    return *this;
}

// value_type op measurement
template<class Y>
inline
measurement<Y>
operator+(Y lhs,const measurement<Y>& rhs)
{
    return (measurement<Y>(lhs,Y(0))+=rhs);
}

template<class Y>
inline
measurement<Y>
operator-(Y lhs,const measurement<Y>& rhs)
{
    return (measurement<Y>(lhs,Y(0))-=rhs);
}

template<class Y>
inline
measurement<Y>
operator*(Y lhs,const measurement<Y>& rhs)
{
    return (measurement<Y>(lhs,Y(0))*=rhs);
}

template<class Y>
inline
measurement<Y>
operator/(Y lhs,const measurement<Y>& rhs)
{
    return (measurement<Y>(lhs,Y(0))/=rhs);
}

// measurement op value_type
template<class Y>
inline
measurement<Y>
operator+(const measurement<Y>& lhs,Y rhs)
{
    return (measurement<Y>(lhs)+=measurement<Y>(rhs,Y(0)));
}

template<class Y>
inline
measurement<Y>
operator-(const measurement<Y>& lhs,Y rhs)
{
    return (measurement<Y>(lhs)-=measurement<Y>(rhs,Y(0)));
}

template<class Y>
inline
measurement<Y>
operator*(const measurement<Y>& lhs,Y rhs)
{
    return (measurement<Y>(lhs)*=measurement<Y>(rhs,Y(0)));
}

template<class Y>
inline
measurement<Y>
operator/(const measurement<Y>& lhs,Y rhs)
{
    return (measurement<Y>(lhs)/=measurement<Y>(rhs,Y(0)));
}

// measurement op measurement
template<class Y>
inline
measurement<Y>
operator+(const measurement<Y>& lhs,const measurement<Y>& rhs)
{
    return (measurement<Y>(lhs)+=rhs);
}

template<class Y>
inline
measurement<Y>
operator-(const measurement<Y>& lhs,const measurement<Y>& rhs)
{
    return (measurement<Y>(lhs)-=rhs);
}

template<class Y>
inline
measurement<Y>
operator*(const measurement<Y>& lhs,const measurement<Y>& rhs)
{
    return (measurement<Y>(lhs)*=rhs);
}

template<class Y>
inline
measurement<Y>
operator/(const measurement<Y>& lhs,const measurement<Y>& rhs)
{
    return (measurement<Y>(lhs)/=rhs);
}

/// specialize power typeof helper
template<class Y,long N,long D>
struct power_typeof_helper<measurement<Y>,static_rational<N,D> >
{
    typedef measurement<
        typename power_typeof_helper<Y,static_rational<N,D> >::type
    > type;

    static type value(const measurement<Y>& x)
    {
        const static_rational<N,D>  rat;

        const Y m = Y(rat.numerator())/Y(rat.denominator()),
                newval = std::pow(x.value(),m),
                err = newval*std::sqrt(std::pow(m*x.uncertainty()/x.value(),2));

        return type(newval,err);
    }
};

/// specialize root typeof helper
template<class Y,long N,long D>
struct root_typeof_helper<measurement<Y>,static_rational<N,D> >
{
    typedef measurement<
        typename root_typeof_helper<Y,static_rational<N,D> >::type
    > type;

    static type value(const measurement<Y>& x)
    {
        const static_rational<N,D>  rat;

        const Y m = Y(rat.denominator())/Y(rat.numerator()),
                newval = std::pow(x.value(),m),
                err = newval*std::sqrt(std::pow(m*x.uncertainty()/x.value(),2));

        return type(newval,err);
    }
};

// stream output
template<class Y>
inline
std::ostream& operator<<(std::ostream& os,const measurement<Y>& val)
{
    boost::io::ios_precision_saver precision_saver(os);
    boost::io::ios_flags_saver flags_saver(os);

    os << val.value() << "(+/-" << val.uncertainty() << ")";

    return os;
}

} // namespace units

} // namespace boost

#endif // BOOST_UNITS_MEASUREMENT_HPP


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

// @sect3{Include files}

// 下面的头文件以前都讨论过了。

#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/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/manifold_lib.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/matrix_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/vector_tools.h>

#include <iostream>
#include <fstream>

// 这个头文件包含了ParameterHandler类的必要声明，我们将用它来从配置文件中读取我们的参数。

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

// 为了解决线性系统，我们将使用UMFPACK提供的稀疏LU分解（见SparseDirectUMFPACK类），为此需要以下头文件。 请注意，为了编译这个教程程序，deal.II-library需要在UMFPACK支持下构建，默认情况下是启用的。

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

// FESystem类允许我们将多个FE对象堆叠成一个复合的、矢量值的有限元场。该类的必要声明在该头文件中提供。

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

// 最后，包括声明定时器类的头文件，我们将用它来确定我们程序的每个操作需要多少时间。

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

// 作为本程序开始时的最后一步，我们将本程序中的所有内容放入其命名空间，并在其中使deal.II命名空间中的所有内容全局可用，不需要在所有内容前加上 dealii::  。

namespace Step29
{
  using namespace dealii;
// @sect3{The DirichletBoundaryValues class}

// 首先我们为代表Dirichlet边界值的函数定义一个类。这在以前已经做过很多次了，因此不需要过多解释。

// 由于有两个值 $v$ 和 $w$ 需要在边界处规定，我们必须告诉基类这是一个有两个分量的向量值函数， vector_value 函数和它的表亲 vector_value_list 必须返回有两个条目的向量。在我们的例子中，这个函数非常简单，它只是对实部 $v$ 返回1，对虚部 $w$ 返回0，而不管它在哪个点被评估。

  template <int dim>
  class DirichletBoundaryValues : public Function<dim>
  {
  public:
    DirichletBoundaryValues()
      : Function<dim>(2)
    {}

    virtual void vector_value(const Point<dim> & /*p*/,
                              Vector<double> &values) const override
    {
      Assert(values.size() == 2, ExcDimensionMismatch(values.size(), 2));

      values(0) = 1;
      values(1) = 0;
    }

    virtual void
    vector_value_list(const std::vector<Point<dim>> &points,
                      std::vector<Vector<double>> &  value_list) const override
    {
      Assert(value_list.size() == points.size(),
             ExcDimensionMismatch(value_list.size(), points.size()));

      for (unsigned int p = 0; p < points.size(); ++p)
        DirichletBoundaryValues<dim>::vector_value(points[p], value_list[p]);
    }
  };
// @sect3{The ParameterReader class}

// 下一个类负责准备ParameterHandler对象并从输入文件中读取参数。 它包括一个声明所有必要参数的函数 declare_parameters 和一个从外部调用的 read_parameters 函数，以启动参数读取过程。

  class ParameterReader : public Subscriptor
  {
  public:
    ParameterReader(ParameterHandler &);
    void read_parameters(const std::string &);

  private:
    void              declare_parameters();
    ParameterHandler &prm;
  };

// 构造函数存储了一个传递给它的ParameterHandler对象的引用。

  ParameterReader::ParameterReader(ParameterHandler &paramhandler)
    : prm(paramhandler)
  {}
// @sect4{ParameterReader::declare_parameters}

//  declare_parameters 函数声明了我们的ParameterHandler对象能够从输入文件中读取的所有参数，以及它们的类型、范围条件和它们出现在哪个分段。我们将用一对大括号包住所有进入一个部分的条目，以迫使编辑器将它们缩进一级，使其更容易阅读哪些条目共同构成一个部分。

  void ParameterReader::declare_parameters()
  {

// 网格和几何参数包括应用于初始粗略网格的全局细化步数和换能器镜头的焦距 $d$ 。对于细化步数，我们允许在 $[0,\infty)$ 范围内的整数，其中 Patterns::Integer 对象的第二个参数被省略，表示半开区间。 对于焦距，任何大于零的数字都可以接受。

    prm.enter_subsection("Mesh & geometry parameters");
    {
      prm.declare_entry("Number of refinements",
                        "6",
                        Patterns::Integer(0),
                        "Number of global mesh refinement steps "
                        "applied to initial coarse grid");

      prm.declare_entry("Focal distance",
                        "0.3",
                        Patterns::Double(0),
                        "Distance of the focal point of the lens "
                        "to the x-axis");
    }
    prm.leave_subsection();

// 下一小节专门讨论方程中出现的物理参数，它们是频率  $\omega$  和波速  $c$  。同样，两者都需要位于半开区间 $[0,\infty)$ 内，通过调用 Patterns::Double 类，仅以左端点为参数来表示。

    prm.enter_subsection("Physical constants");
    {
      prm.declare_entry("c", "1.5e5", Patterns::Double(0), "Wave speed");

      prm.declare_entry("omega", "5.0e7", Patterns::Double(0), "Frequency");
    }
    prm.leave_subsection();

// 最后但并非最不重要的是，我们希望能够通过配置文件中的条目来改变输出的一些属性，如文件名和格式，这就是最后一小节的目的。

    prm.enter_subsection("Output parameters");
    {
      prm.declare_entry("Output filename",
                        "solution",
                        Patterns::Anything(),
                        "Name of the output file (without extension)");

// 由于不同的输出格式在生成输出时可能需要不同的参数（例如，postscript输出需要视角角度、线宽、颜色等），如果我们必须为库中支持的每一种可能的输出格式手工声明所有这些参数，那就太麻烦了。相反，每种输出格式都有一个 FormatFlags::declare_parameters 函数，它在自己的小节中声明了该格式的所有特定参数。下面调用 DataOutInterface<1>::declare_parameters 为所有可用的输出格式执行 declare_parameters ，这样就为每一种格式创建了一个自己的小节，为该特定的输出格式声明参数。(上面 @<1@> 的调用中，模板参数的实际值在这里并不重要：该函数做了同样的工作，与维度无关，但恰好是在一个依赖模板参数的类中。)  要想知道哪种输出格式有哪些参数，你可以查阅DataOutBase类的文档，或者干脆在没有参数文件的情况下运行这个程序。然后它将创建一个文件，其中所有声明的参数都设置为默认值，这可以方便地作为一个起点，将参数设置为你想要的值。

      DataOutInterface<1>::declare_parameters(prm);
    }
    prm.leave_subsection();
  }
// @sect4{ParameterReader::read_parameters}

// 这是ParameterReader类中的主函数。 它从外部被调用，首先声明所有的参数，然后从输入文件中读取参数，文件名由调用者提供。对这个函数的调用完成后，可以用 prm 对象来检索从文件中读入的参数值。

  void ParameterReader::read_parameters(const std::string &parameter_file)
  {
    declare_parameters();

    prm.parse_input(parameter_file);
  }

//  @sect3{The ComputeIntensity class}

// 正如介绍中所提到的，我们真正追求的量是超声波强度的空间分布，它对应于  $|u|=\sqrt{v^2+w^2}$  。现在我们可以只满足于在输出中拥有 $v$ 和 $w$ ，并使用合适的可视化或后处理工具从我们计算的解决方案中得出 $|u|$ 。然而，也有一种方法可以输出从deal.II中的解决方案中得出的数据，我们在这里要利用这个机制。

// 到目前为止，我们一直使用 DataOut::add_data_vector 函数将包含输出数据的向量添加到一个DataOut对象中。 这个函数有一个特殊的版本，除了数据向量之外，还有一个额外的参数类型为DataPostprocessor。当这个函数用于输出时，在每个要生成输出数据的点上，指定的DataPostprocessor对象的 DataPostprocessor::evaluate_scalar_field() 或 DataPostprocessor::evaluate_vector_field() 函数被调用，从数据向量代表的有限元函数的值、梯度和二阶导数计算输出量（在面相关数据的情况下，法向量也是可用的）。因此，这使我们可以输出任何可以从解的值及其导数中局部导出的数量。 当然，超声强度 $|u|$ 就是这样一个量，它的计算甚至不涉及任何 $v$ 或 $w$ 的导数。

// 在实践中，DataPostprocessor类只提供了这个功能的接口，我们需要从它派生出我们自己的类，以实现接口所指定的功能。在最一般的情况下，我们必须实现几个成员函数，但是如果输出量是一个单一的标量，那么其中的一些模板代码可以由一个更专业的类DataPostprocessorScalar来处理，我们可以从这个类派生。这就是 ComputeIntensity 类的作用。

  template <int dim>
  class ComputeIntensity : public DataPostprocessorScalar<dim>
  {
  public:
    ComputeIntensity();

    virtual void evaluate_vector_field(
      const DataPostprocessorInputs::Vector<dim> &inputs,
      std::vector<Vector<double>> &computed_quantities) const override;
  };

// 在构造函数中，我们需要用两个参数调用基类的构造函数。第一个参数表示由该类计算的单一标量在输出文件中应表示的名称。在我们的例子中，后处理程序有 $|u|$ 作为输出，所以我们使用 "Intensity"。

// 第二个参数是一组标志，表示后处理程序需要哪些数据来计算输出量。 这可以是update_values、update_gradients和update_hessians（如果是脸部数据，也可以是update_normal_vector）的任何一个子集，这些都在UpdateFlags中记录。 当然，导数的计算需要额外的资源，所以这里只应该给出真正需要的数据的标志，就像我们使用FEValues对象时一样。在我们的例子中，只有  $v$  和  $w$  的函数值需要用来计算  $|u|$  ，所以我们用 update_values 标志就可以了。

  template <int dim>
  ComputeIntensity<dim>::ComputeIntensity()
    : DataPostprocessorScalar<dim>("Intensity", update_values)
  {}

// 实际的后处理发生在下面这个函数中。它的输入是一个存储函数值的对象（这里是向量值），代表给 DataOut::add_data_vector, 的数据向量在我们产生输出的所有评估点的评估值，以及一些代表导数的张量对象（我们在这里没有使用，因为 $|u|$ 只是从 $v$ 和 $w$ 计算出来）。派生量在 computed_quantities 向量中返回。请记住，这个函数只能使用由  get_needed_update_flags  指定的各自更新标志的数据。例如，我们可能不会在这里使用导数，因为我们对  get_needed_update_flags  的实现要求只提供函数值。

  template <int dim>
  void ComputeIntensity<dim>::evaluate_vector_field(
    const DataPostprocessorInputs::Vector<dim> &inputs,
    std::vector<Vector<double>> &               computed_quantities) const
  {
    Assert(computed_quantities.size() == inputs.solution_values.size(),
           ExcDimensionMismatch(computed_quantities.size(),
                                inputs.solution_values.size()));

// 计算本身是很简单的。我们遍历输出向量中的每个条目，并从 $v$ 和 $w$ 的相应值中计算出 $|u|$ 。我们通过创建一个复数 $u$ ，然后对结果调用 `std::abs()` 来实现。(我们可能想调用 `std::norm()`, ，但是在一个历史的怪圈中，C++委员会决定 `std::norm()` 应该返回绝对值的square--从而不满足数学家对所谓 "规范 "的属性要求。)

    for (unsigned int i = 0; i < computed_quantities.size(); i++)
      {
        Assert(computed_quantities[i].size() == 1,
               ExcDimensionMismatch(computed_quantities[i].size(), 1));
        Assert(inputs.solution_values[i].size() == 2,
               ExcDimensionMismatch(inputs.solution_values[i].size(), 2));

        const std::complex<double> u(inputs.solution_values[i](0),
                                     inputs.solution_values[i](1));

        computed_quantities[i](0) = std::abs(u);
      }
  }
// @sect3{The UltrasoundProblem class}

// 最后这里是这个程序的主类。 它的成员函数与前面的例子非常相似，特别是 step-4 ，成员变量的列表也没有什么大的惊喜。传递给构造函数的ParameterHandler对象被存储为一个引用，以便于从该类的所有函数中访问参数。 由于我们正在使用矢量值的有限元，我们使用的FE对象是FESystem类型的。

  template <int dim>
  class UltrasoundProblem
  {
  public:
    UltrasoundProblem(ParameterHandler &);
    void run();

  private:
    void make_grid();
    void setup_system();
    void assemble_system();
    void solve();
    void output_results() const;

    ParameterHandler &prm;

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

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;
    Vector<double>       solution, system_rhs;
  };

// 构造函数接收ParameterHandler对象并将其存储在一个引用中。它还初始化了DoF-Handler和有限元系统，该系统由标量Q1场的两个副本组成，一个用于 $v$ ，一个用于 $w$  。

  template <int dim>
  UltrasoundProblem<dim>::UltrasoundProblem(ParameterHandler &param)
    : prm(param)
    , dof_handler(triangulation)
    , fe(FE_Q<dim>(1), 2)
  {}
// @sect4{UltrasoundProblem::make_grid}

// 这里我们为我们的领域设置网格。 正如论述中所提到的，这个几何体只是一个单位正方形（2d），其边界部分代表换能器透镜，由一个圆的扇形代替。

  template <int dim>
  void UltrasoundProblem<dim>::make_grid()
  {

// 首先我们生成一些日志输出，并启动一个定时器，这样我们就可以在这个函数完成后计算出执行时间。

    std::cout << "Generating grid... ";
    Timer timer;

// 然后，我们从ParameterHandler对象中查询换能器镜头的焦距和网格细化步数的值。

    prm.enter_subsection("Mesh & geometry parameters");

    const double       focal_distance = prm.get_double("Focal distance");
    const unsigned int n_refinements = prm.get_integer("Number of refinements");

    prm.leave_subsection();

// 接下来，为换能器镜头的位置和焦点定义了两个点，也就是圆的中心，其线段将形成边界的换能器部分。注意，这是程序中唯一一个在二维和三维中略有不同的地方。尽管本教程只涉及二维情况，但要使这个程序在三维中发挥作用，必要的补充是非常少的，所以我们选择包括它们。

    const Point<dim> transducer =
      (dim == 2) ? Point<dim>(0.5, 0.0) : Point<dim>(0.5, 0.5, 0.0);
    const Point<dim> focal_point = (dim == 2) ?
                                     Point<dim>(0.5, focal_distance) :
                                     Point<dim>(0.5, 0.5, focal_distance);

// 作为初始粗网格，我们采用一个简单的单位正方形，每个方向上有5个细分。分区的数量是这样选择的：我们想指定为传感器边界的线段 $[0.4,0.6]$ 是由一个面来跨越的。然后，我们通过所有的单元格，找到换能器所在的面，事实上，这只是X轴上从0.4到0.6的一条边。这是我们希望根据圆环形边界进行细化的地方，所以我们用不同的流形指标来标记这个边缘。由于我们要在换能器上设置迪里希特边界条件，所以我们也要改变其边界指标。

    GridGenerator::subdivided_hyper_cube(triangulation, 5, 0, 1);

    for (auto &cell : triangulation.cell_iterators())
      for (const auto &face : cell->face_iterators())
        if (face->at_boundary() &&
            ((face->center() - transducer).norm_square() < 0.01))
          {
            face->set_boundary_id(1);
            face->set_manifold_id(1);
          }

// 对于换能器镜头的圆形部分，使用了一个SphericalManifold对象（当然，在2D中只是代表一个圆），中心的计算方法如上。

    triangulation.set_manifold(1, SphericalManifold<dim>(focal_point));

// 现在，全局细化被执行。靠近换能器位置的单元格将根据换能器透镜的圆形边界被自动细化。

    triangulation.refine_global(n_refinements);

// 最后，我们再生成一些日志输出。我们停止定时器，并查询自函数开始以来所经过的CPU秒数。

    timer.stop();
    std::cout << "done (" << timer.cpu_time() << "s)" << std::endl;

    std::cout << "  Number of active cells:  " << triangulation.n_active_cells()
              << std::endl;
  }
// @sect4{UltrasoundProblem::setup_system}

// 系统矩阵的初始化、稀疏模式和向量与前面的例子相同，因此不需要进一步评论。和前面的函数一样，我们也输出我们在这里所做的运行时间。

  template <int dim>
  void UltrasoundProblem<dim>::setup_system()
  {
    std::cout << "Setting up system... ";
    Timer timer;

    dof_handler.distribute_dofs(fe);

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

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

    timer.stop();
    std::cout << "done (" << timer.cpu_time() << "s)" << std::endl;

    std::cout << "  Number of degrees of freedom: " << dof_handler.n_dofs()
              << std::endl;
  }
// @sect4{UltrasoundProblem::assemble_system}

// 和以前一样，这个函数负责组装系统矩阵和右手边的向量。

  template <int dim>
  void UltrasoundProblem<dim>::assemble_system()
  {
    std::cout << "Assembling system matrix... ";
    Timer timer;

// 首先我们从ParameterHandler对象中查询波速和频率，并将其存储在本地变量中，因为它们将在本函数中频繁使用。

    prm.enter_subsection("Physical constants");

    const double omega = prm.get_double("omega"), c = prm.get_double("c");

    prm.leave_subsection();

// 像往常一样，计算积分时使用普通的高斯正交规则。由于我们的双线性形式涉及到 $\Gamma_2$ 上的边界积分，所以我们还需要一个正交法则来计算面的积分，这些面是 $dim-1$ 维的。

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

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

// FEValues对象将为我们评估形状函数。 对于涉及到 $\Omega$ 上的积分的双线性形式的部分，我们需要形状函数的值和梯度，当然还有正交权重。 对于涉及边界积分的条款，只需要形状函数值和正交权重。

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

    FEFaceValues<dim> fe_face_values(fe,
                                     face_quadrature_formula,
                                     update_values | update_JxW_values);

// 像往常一样，系统矩阵是逐个单元组装的，我们需要一个矩阵来存储本地单元的贡献，以及一个索引向量来将单元的贡献转移到全局系统矩阵中的适当位置，然后。

    FullMatrix<double> cell_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())
      {

// 在每个单元，我们首先需要重置本地贡献矩阵，并请求FEValues对象计算当前单元的形状函数。

        cell_matrix = 0;
        fe_values.reinit(cell);

        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          {
            for (unsigned int j = 0; j < dofs_per_cell; ++j)
              {

// 在这一点上，重要的是要记住，我们所处理的是一个有两个组成部分的有限元系统。由于我们构造这个FESystem的方式，即作为两个标量有限元场的笛卡尔乘积，每个形状函数只有一个非零分量（用deal.II的行话说，它们是 @ref  GlossPrimitive "原始"）。 因此，每个形状函数可以被看作是引言中的 $\phi$ 's或 $\psi$ 's之一，同样，相应的自由度也可以归属于 $\alpha$ 或 $\beta$  。      然而，当我们遍历当前单元上的所有自由度时，它们并不以任何特定的顺序出现，因此我们无法立即决定索引为 $i$ 和 $j$ 的自由度是属于我们解决方案的实部还是虚部。 另一方面，如果你看一下介绍中的系统矩阵的形式，这个区别是至关重要的，因为它将决定当前一对DoF的贡献将归入系统矩阵的哪个块，因此我们需要从给定的两个形状函数中计算哪个数量。 幸运的是，FESystem对象可以为我们提供这些信息，即它有一个函数 FESystem::system_to_component_index, ，为每个局部的DoF索引返回一对整数，其中第一个表示该DoF属于系统的哪个组成部分。这对整数中的第二个整数表示该DoF在标量基有限元场中的索引，但这一信息在这里并不相关。如果你想知道更多关于这个函数和原始向量值元素背后的基本方案，可以看看 step-8 或 @ref vector_valued 模块，那里对这些主题有深入的解释。

                if (fe.system_to_component_index(i).first ==
                    fe.system_to_component_index(j).first)
                  {

// 如果DoF $i$ 和 $j$ 都属于同一个分量，即它们的形状函数都是 $\phi$ 的，或者都是 $\psi$ 的，贡献将最终出现在我们系统矩阵的一个对角块中，由于相应的条目是由同一个公式计算的，我们不必理会它们实际上是 $\phi$ 还是 $\psi$ 形状函数。我们可以简单地通过遍历所有正交点并将其贡献相加来计算条目，其中形状函数的值和梯度由我们的FEValues对象提供。

                    for (unsigned int q_point = 0; q_point < n_q_points;
                         ++q_point)
                      cell_matrix(i, j) +=
                        (((fe_values.shape_value(i, q_point) *
                           fe_values.shape_value(j, q_point)) *
                            (-omega * omega) +
                          (fe_values.shape_grad(i, q_point) *
                           fe_values.shape_grad(j, q_point)) *
                            c * c) *
                         fe_values.JxW(q_point));

// 你可能认为我们在向FEValues对象请求形状函数值或梯度时，必须指定我们想评估的形状函数的哪个分量。然而，由于形状函数是原始的，它们只有一个非零分量，而且FEValues类足够聪明，它知道我们肯定对这一个非零分量感兴趣。

                  }
              }
          }

// 我们还必须增加由于边界项的贡献。为此，我们对当前单元格的所有面进行循环，首先看它是否在边界上，其次看它是否有与 $\Gamma_2$ 相关的正确的边界指标，即我们有吸收边界条件的那部分边界。

        for (const auto face_no : cell->face_indices())
          if (cell->face(face_no)->at_boundary() &&
              (cell->face(face_no)->boundary_id() == 0))
            {

// 这些面肯定会对系统矩阵的非对角线块作出贡献，所以我们要求FEFaceValues对象为我们提供这个面上的形状函数值。

              fe_face_values.reinit(cell, face_no);

// 接下来，我们循环浏览当前单元格的所有DoF，找到属于不同组件且都支持当前面号的一对。

              for (unsigned int i = 0; i < dofs_per_cell; ++i)
                for (unsigned int j = 0; j < dofs_per_cell; ++j)
                  if ((fe.system_to_component_index(i).first !=
                       fe.system_to_component_index(j).first) &&
                      fe.has_support_on_face(i, face_no) &&
                      fe.has_support_on_face(j, face_no))

// 检查形状函数在一个面上是否有支持并不是严格必要的：如果我们不检查它，我们会简单地把本地单元矩阵的项加起来，这些项碰巧是零，因为至少有一个形状函数碰巧是零。然而，我们可以通过添加上面的检查来节省这项工作。

// 在任何一种情况下，这些DoFs都会对系统矩阵的对角线区块的边界积分作出贡献。为了计算积分，我们在面的所有正交点上进行循环，然后用面的正交规则提供的正交权重对贡献进行加权求和。 与对角线块上的条目不同，这里的形状函数哪一个是 $\psi$ ，哪一个是 $\phi$ ，确实很重要，因为这将决定该条目的符号。 我们通过一个简单的条件语句来说明这一点，它决定了正确的符号。由于我们已经检查了DoF  $i$  和  $j$  属于不同的组件，这里只需测试其中一个组件属于哪个组件即可。

                    for (unsigned int q_point = 0; q_point < n_face_q_points;
                         ++q_point)
                      cell_matrix(i, j) +=
                        ((fe.system_to_component_index(i).first == 0) ? -1 :
                                                                        1) *
                        fe_face_values.shape_value(i, q_point) *
                        fe_face_values.shape_value(j, q_point) * c * omega *
                        fe_face_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));
      }

// 唯一剩下的是 $\Gamma_1$ 上的迪里希特边界值，其特征是边界指标1。Dirichlet值由我们上面定义的 DirichletBoundaryValues 类提供。

    std::map<types::global_dof_index, double> boundary_values;
    VectorTools::interpolate_boundary_values(dof_handler,
                                             1,
                                             DirichletBoundaryValues<dim>(),
                                             boundary_values);

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

    timer.stop();
    std::cout << "done (" << timer.cpu_time() << "s)" << std::endl;
  }

//  @sect4{UltrasoundProblem::solve}

// 正如介绍中已经提到的，系统矩阵既不是对称的，也不是确定的，因此，如何提出一个迭代求解器和预处理器来很好地处理这个矩阵并不是很明显。 我们选择了另一种方式，用UMFPACK提供的稀疏LU分解来解决线性系统。这通常是二维问题的一个很好的首选，即使对于大量的DoF也能很好地工作。 SparseDirectUMFPACK类提供了UMFPACK的deal.II接口，它非常容易使用，使我们仅用3行代码就能解决我们的线性系统。

// 再次注意，为了编译这个例子程序，你需要有支持UMFPACK的deal.II库。

  template <int dim>
  void UltrasoundProblem<dim>::solve()
  {
    std::cout << "Solving linear system... ";
    Timer timer;

// 解决线性系统的代码很短：首先，我们分配一个正确类型的对象。下面的 initialize 调用提供了我们想要反转的矩阵给SparseDirectUMFPACK对象，并同时启动了LU分解。因此，这也是这个程序中大部分计算工作发生的地方。

    SparseDirectUMFPACK A_direct;
    A_direct.initialize(system_matrix);

// 分解之后，我们可以把 A_direct 当作代表我们系统矩阵的逆矩阵来使用，所以要计算出解决方案，我们只需要与右边的向量相乘。

    A_direct.vmult(solution, system_rhs);

    timer.stop();
    std::cout << "done (" << timer.cpu_time() << "s)" << std::endl;
  }

//  @sect4{UltrasoundProblem::output_results}

// 这里我们按照参数文件中指定的格式输出我们的解 $v$ 和 $w$ 以及导出的量 $|u|$ 。从 $v$ 和 $w$ 导出 $|u|$ 的大部分工作已经在 ComputeIntensity 类的实现中完成，因此输出程序相当简单，与前面教程中的内容非常相似。

  template <int dim>
  void UltrasoundProblem<dim>::output_results() const
  {
    std::cout << "Generating output... ";
    Timer timer;

// 定义我们的  ComputeIntensity  类的对象和一个 DataOut 对象。

    ComputeIntensity<dim> intensities;
    DataOut<dim>          data_out;

    data_out.attach_dof_handler(dof_handler);

// 接下来我们从ParameterHandler查询输出相关的参数。 DataOut::parse_parameters  调用作为  ParameterReader::declare_parameters  中  DataOutInterface<1>::declare_parameters  调用的对应方。它从ParameterHandler收集所有与输出格式相关的参数，并相应地设置DataOut对象的相应属性。

    prm.enter_subsection("Output parameters");

    const std::string output_filename = prm.get("Output filename");
    data_out.parse_parameters(prm);

    prm.leave_subsection();

// 现在，我们从ParameterHandler提供的基本名称和DataOut类提供的后缀（默认后缀被设置为正确的类型，与.prm文件中通过parse_parameters()设置的后缀相匹配）来组合文件名。

    const std::string filename = output_filename + data_out.default_suffix();

    std::ofstream output(filename);

//解向量 $v$ 和 $w$ 以常规方式添加到DataOut对象中。

    std::vector<std::string> solution_names;
    solution_names.emplace_back("Re_u");
    solution_names.emplace_back("Im_u");

    data_out.add_data_vector(solution, solution_names);

// 对于强度，我们只是再次调用 add_data_vector ，但这次用我们的 ComputeIntensity 对象作为第二个参数，这实际上是将 $|u|$ 加入到输出数据中。

    data_out.add_data_vector(solution, intensities);

// 最后的步骤和以前一样。注意，现在实际的输出格式是由输入文件中的内容决定的，也就是说，人们可以改变输出格式而不必重新编译这个程序。

    data_out.build_patches();
    data_out.write(output);

    timer.stop();
    std::cout << "done (" << timer.cpu_time() << "s)" << std::endl;
  }

//  @sect4{UltrasoundProblem::run}

// 这里我们简单地一个接一个地执行我们的函数。

  template <int dim>
  void UltrasoundProblem<dim>::run()
  {
    make_grid();
    setup_system();
    assemble_system();
    solve();
    output_results();
  }
} // namespace Step29
// @sect4{The main function}

// 最后是该程序的 main 功能。它的结构与其他几乎所有的教程程序相同。唯一的例外是，我们定义了ParameterHandler和 ParameterReader 对象，并让后者从一个名为 step-29.prm 的文本文件中读入参数值。这样读到的值会被交给UltrasoundProblem类的一个实例。

int main()
{
  try
    {
      using namespace dealii;
      using namespace Step29;

      ParameterHandler prm;
      ParameterReader  param(prm);
      param.read_parameters("step-29.prm");

      UltrasoundProblem<2> ultrasound_problem(prm);
      ultrasound_problem.run();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  return 0;
}


#include <iostream>
#include <Eigen/Dense>
#include "simulator.h"

using namespace std;
using namespace Eigen;
using namespace std::chrono;

int main() {
	cout << "Solving multi-robot motion planning problem..." << endl;

    std::ifstream my_config_file("../config/config.json");
    assert(my_config_file && "Couldn't find the config file");

    Simulator sim(my_config_file);

    int T = 20; // simulation duration
    sim.run(T);

    // Save data to file
    string file = "../results/trajectories.txt";
    sim.saveDataToFile(file.c_str());

	return 0;
}


#pragma once

#include <cassert>
#include <chrono>
#include <random>
#include <type_traits>
#include <vector>

#include <boost/range/adaptor/filtered.hpp>
#include <boost/range/empty.hpp>
#include <boost/range/size.hpp>
#include <boost/range/algorithm/sort.hpp>
#include <boost/range/algorithm_ext/iota.hpp>

namespace elle
{
  /// A unique random device.
  std::mt19937&
  random_engine();

  /// A uniform distribution in [0, size(r) - 1].
  ///
  /// @pre c is not empty.
  template <typename Range>
  auto
  uniform_index_distribution(Range const& r)
  {
    assert(!boost::empty(r));
    return std::uniform_int_distribution<>(0, boost::size(r) - 1);
  }

  /// Random integer in [0, size-1].
  ///
  /// @return an iterator.
  template <typename Integral, typename Gen = std::mt19937>
  auto
  pick_one(Integral size, Gen& gen = random_engine())
    -> std::enable_if_t<std::is_integral<Integral>{}, Integral>
  {
    auto random = std::uniform_int_distribution<Integral>(0, size - 1);
    return random(gen);
  }

  /// Random duration in [0, d[.
  template <typename Repr, typename Ratio, typename Gen = std::mt19937>
  std::chrono::duration<Repr, Ratio>
  pick_one(std::chrono::duration<Repr, Ratio> const& d, Gen& gen = random_engine())
  {
    return std::chrono::duration<Repr, Ratio>{pick_one(d.count(), gen)};
  }

  /// Random selection in a range.
  ///
  /// @return an iterator.
  template <typename Range, typename Gen = std::mt19937>
  auto
  pick_one(Range& r, Gen& gen = random_engine())
    -> decltype(gen.seed(0),  // sfinae
                boost::begin(r))
  {
    if (boost::empty(r))
      return r.end();
    else
      return std::next(r.begin(), pick_one(boost::size(r), gen));
  }

  /// Random selection in a range with a filter.
  ///
  /// @return an iterator.
  template <typename Range, typename Pred, typename Gen = std::mt19937>
  auto
  pick_one(Range& r, Pred pred, Gen& gen = random_engine())
    -> decltype(pred(*boost::begin(r)),  // sfinae
                boost::begin(r))
  {
    auto&& filtered = boost::adaptors::filter(r, pred);
    return pick_one(filtered, gen).base();
  }

  /// A (sorted) range of n numbers to choose in [0, size - 1].
  ///
  /// @pre n < size(r).
  template <typename Gen = std::mt19937>
  auto
  pick_n(int count, int size, Gen& gen = random_engine())
  {
    assert(count <= size);
    auto res = std::vector<int>{};
    if (count < size)
    {
      while (res.size() < static_cast<unsigned int>(count))
      {
        auto random = std::uniform_int_distribution<>(0, size - 1 - res.size());
        int v = random(gen);
        for (auto r: res)
          if (v >= r)
            ++v;
        res.push_back(v);
        boost::sort(res);
      }
    }
    else
    {
      res.resize(size);
      boost::iota(res, 0);
    }
    return res;
  }

  /// From a list of indexes in r, make a list of iterators.
  template <typename Range, typename Indexes>
  auto
  select(Range& r, Indexes const& is)
  {
    using Iterator = decltype(boost::begin(r));
    auto res = std::vector<Iterator>{};
    res.reserve(boost::size(is));
    // Don't require direct access iterators.
    auto it = r.begin();
    auto prev = 0;
    for (auto i: is)
    {
      std::advance(it, i - prev);
      prev = i;
      res.push_back(it);
    }
    return res;
  }

  /// A (sorted) range of n iterators in a container.
  ///
  /// @pre n < size(r).
  template <typename Range, typename Gen = std::mt19937>
  auto
  pick_n(int count, Range& r, Gen& gen = random_engine())
  {
    return select(r, pick_n(count, boost::size(r), gen));
  }
}


/*=============================================================================
  Copyright (c) 2001-2007 Joel de Guzman

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

#include <iostream>
#include <cmath>
#include <boost/detail/lightweight_test.hpp>
#include <boost/phoenix/core.hpp>
#include <boost/phoenix/operator.hpp>
#include <boost/phoenix/function.hpp>

#define BOOST_MPL_CFG_NO_PREPROCESSED_HEADERS
#include <boost/mpl/multiplies.hpp>
#undef BOOST_MPL_CFG_NO_PREPROCESSED_HEADERS

struct test_impl
{
    typedef void result_type;
    void operator()() const
    {
        std::cout << "Test lazy functions...\n";
    }
};

boost::phoenix::function<test_impl> test;

struct sqr_impl
{
    template <typename Signature>
    struct result;

    template <typename This, typename Arg>
    struct result<This(Arg)>
    {
        typedef typename boost::remove_reference<Arg>::type type;
    };

    template <typename Arg>
    Arg
    operator()(Arg n) const
    {
        return n * n;
    }
};

boost::phoenix::function<sqr_impl> sqr;

struct fact_impl
{
    template <typename Signature>
    struct result;

    template <typename This, typename Arg>
    struct result<This(Arg)>
    {
        typedef typename boost::remove_reference<Arg>::type type;
    };

    template <typename Arg>
    Arg
    operator()(Arg n) const
    {
        return (n <= 0) ? 1 : n * (*this)(n-1);
    }
};

boost::phoenix::function<fact_impl> fact;

struct pow_impl
{

    template <typename Sig>
    struct result;

    template <typename This, typename Arg1, typename Arg2>
    struct result<This(Arg1, Arg2)>
    {
        typedef typename boost::remove_reference<Arg1>::type type;
    };

    template <typename Arg1, typename Arg2>
    Arg1
    operator()(Arg1 a, Arg2 b) const
    {
      return std::pow(a, b);
    }
};

boost::phoenix::function<pow_impl> power;

struct add_impl
{
    template <typename Sig>
    struct result;

    template <typename This, typename Arg1, typename Arg2, typename Arg3, typename Arg4>
    struct result<This(Arg1, Arg2, Arg3, Arg4)>
    {
        typedef typename boost::remove_reference<Arg1>::type type;
    };

    template <typename Arg1, typename Arg2, typename Arg3, typename Arg4>
    Arg1
    operator()(Arg1 a, Arg2 b, Arg3 c, Arg4 d) const
    {
        return a + b + c + d;
    }
};

boost::phoenix::function<add_impl> add;

int
main()
{
    using boost::phoenix::arg_names::arg1;
    using boost::phoenix::arg_names::arg2;

    int i5 = 5;
    double d5 = 5, d3 = 3;

    test()();
    BOOST_TEST(sqr(arg1)(i5) == (i5*i5));
    BOOST_TEST(fact(4)() == 24);
    BOOST_TEST(fact(arg1)(i5) == 120);
    BOOST_TEST((int)power(arg1, arg2)(d5, d3) == (int)std::pow(d5, d3));
    BOOST_TEST((sqr(arg1) + 5)(i5) == ((i5*i5)+5));
    BOOST_TEST(add(arg1, arg1, arg1, arg1)(i5) == (5+5+5+5));

    // testing composition
    BOOST_TEST(add(arg1, arg1, arg1, power(arg1, 2))(d5) == (5+5+5+25));

    int const ic5 = 5;
    // testing consts
    BOOST_TEST(sqr(arg1)(ic5) == (ic5*ic5));

    return boost::report_errors();
}


#include "../monotone_decomposition.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>

#include <algorithm>

namespace boost {
    namespace test_tools {
        template<>
        struct print_log_value<coordinate>
        {
            void operator()(std::ostream& os,const coordinate& coord)
            {
                ::operator<<(os, coord);
            }
        };
    }
}

BOOST_AUTO_TEST_SUITE(monotone_decomposition_tests)

BOOST_AUTO_TEST_CASE(example_test)
{
    //                      6
    //    1                /
    //   / \              /
    //  /   \            /
    // 0     2  4       /
    //       | / \     /
    //       |/   \   /
    //       3      5
    poly_line line {0,
        std::vector<coordinate> {
            coordinate {0, 0},
            coordinate {1, 1},
            coordinate {2, 0},
            coordinate {2, -1},
            coordinate {3, 0},
            coordinate {4, -1},
            coordinate {5, 2}
        }
    };

    monotone_decomposition decomposition;
    auto monotone_lines = decomposition(line);

    BOOST_CHECK_EQUAL(monotone_lines.size(), 1);
    BOOST_CHECK_EQUAL(monotone_lines[0].line.id, 0);
    BOOST_CHECK_EQUAL(monotone_lines[0].line.coordinates.size(), 7);
    for (auto i = 0u; i < monotone_lines[0].line.coordinates.size(); ++i)
        BOOST_CHECK_EQUAL(line.coordinates[i], monotone_lines[0].line.coordinates[i]);
}

BOOST_AUTO_TEST_CASE(y_monotone_increasing)
{
    //     3
    //    /
    //   /
    //  2
    //   \
    //    \
    //     1
    //    /
    //   /
    //  0
    //
    poly_line line {0,
        std::vector<coordinate> {
            coordinate {0, 0},
            coordinate {1, 1},
            coordinate {0, 2},
            coordinate {1, 3},
        }
    };

    monotone_decomposition decomposition;
    auto monotone_lines = decomposition(line);

    BOOST_CHECK_EQUAL(monotone_lines.size(), 1);
    BOOST_CHECK_EQUAL(monotone_lines[0].line.id, 0);
    BOOST_CHECK_EQUAL(monotone_lines[0].line.coordinates.size(), 4);
    for (auto i = 1u; i < monotone_lines[0].line.coordinates.size(); ++i)
        BOOST_CHECK(monotone_lines[0].line.coordinates[i-1].x <= monotone_lines[0].line.coordinates[i].x);
}

BOOST_AUTO_TEST_CASE(y_monotone_decreasing)
{
    //     0
    //    /
    //   /
    //  1
    //   \
    //    \
    //     2
    //    /
    //   /
    //  3
    //
    poly_line line {0,
        std::vector<coordinate> {
            coordinate {1, 3},
            coordinate {0, 2},
            coordinate {1, 1},
            coordinate {0, 0},
        }
    };

    monotone_decomposition decomposition;
    auto monotone_lines = decomposition(line);

    BOOST_CHECK_EQUAL(monotone_lines.size(), 1);
    BOOST_CHECK_EQUAL(monotone_lines[0].line.id, 0);
    BOOST_CHECK_EQUAL(monotone_lines[0].line.coordinates.size(), 4);
    for (auto i = 1u; i < monotone_lines[0].line.coordinates.size(); ++i)
        BOOST_CHECK(monotone_lines[0].line.coordinates[i-1].x <= monotone_lines[0].line.coordinates[i].x);
}

BOOST_AUTO_TEST_CASE(example_line)
{
    //
    //        4
    //        |
    //        |
    //   2____3
    //   |
    //   1---------0
    //
    std::vector<coordinate> cs = {
        coordinate {2, 0},      // 0
        coordinate {0, 0},      // 1
        coordinate {0, 1},      // 2
        coordinate {1, 1},      // 2
        coordinate {1, 2},      // 3
    };

    std::vector<point> points = {
        {point::NO_LINE_ID, 0, coordinate {2.25, 2.75}}, // a
        {point::NO_LINE_ID, 1, coordinate {2.1, 0.05}},  // b
        {point::NO_LINE_ID, 2, coordinate {0.9, 1.1}},   // c
        {point::NO_LINE_ID, 3, coordinate {1.1, 1.1}},   // d
    };

    poly_line line = {0, {cs[0], cs[1], cs[2], cs[3], cs[4]}};

    monotone_decomposition decomposition;
    auto monotone_lines = decomposition(line);

    BOOST_CHECK_EQUAL(monotone_lines.size(), 1);
    BOOST_CHECK_EQUAL(monotone_lines[0].line.id, 0);
    BOOST_CHECK_EQUAL(monotone_lines[0].line.coordinates.size(), 5);
    for (auto i = 1u; i < monotone_lines[0].line.coordinates.size(); ++i)
        BOOST_CHECK(monotone_lines[0].line.coordinates[i-1].x <= monotone_lines[0].line.coordinates[i].x);
}

BOOST_AUTO_TEST_SUITE_END()


//
// SPDX-License-Identifier: MIT
// Copyright (c) 2016-2020 Michael Purcaro, Henry Pratt, Jill Moore, Zhiping Weng
//

#include <vector>
#include <fstream>
#include <iterator>
#include <string>
#include <cmath>
#include <numeric>
#include <unordered_map>

#include <boost/filesystem.hpp>

#include "zentLib/src/BigWigWrapper.hpp"
#include "cpp/files.hpp"
#include "cpp/string_utils.hpp"

#include "utils.hpp"
#include "lambda.hpp"
#include "region.hpp"
#include "zscore.hpp"

namespace SCREEN {

  float log10orNeg10(float x) {
    // TODO: floating point tolerance?
    // http://realtimecollisiondetection.net/blog/?p=89
    if (0.0 == x){
      return -10.0;
    }
    return std::log10(x + 0.01);
  }

} // SCREEN


/*
 * Taken from http://www.andrew.cmu.edu/user/vanhoeve/mdd/ with the notice:
 * "The software can be freely used but comes with no warranty"
 * solver.hpp
 *
 *  Created on: Jan 28, 2012
 *      Author: Bergman, Cire
 */

#ifndef INDEPSET_SOLVER_HPP_
#define INDEPSET_SOLVER_HPP_

#define EXACT_BDD -1

#include "bdd.hpp"
#include "instance.hpp"
#include "stats.hpp"
#include "intset.hpp"
#include "orderings.hpp"
#include "merge.hpp"

#include <boost/random/discrete_distribution.hpp>
#include <vector>
#include <map>
#include <queue>

using namespace std;

/**
 * Node comparator by relax ub
 */
struct CompareNodesRelaxUB {
  bool operator()(Node* nodeA, Node* nodeB) const {
	  if( nodeA->relax_ub == nodeB->relax_ub ) {
		return( nodeA->state.get_size() > nodeB->state.get_size() );
	  }
	  return nodeA->relax_ub < nodeB->relax_ub;
  }
};

typedef priority_queue<Node*, vector<Node*>, CompareNodesRelaxUB> NodeQueue;

struct Bounds {
	int lb;
	int ub;
};

struct IndepSetSolver {

	NodeMap  						node_list;

	vector<int>						active_vertices;
	int*							in_state_counter;
	int*							active_vertex_map;

	vector<Node*>					nodes_layer;			      /**< nodes in a layer */

	vector<Node*>					temporary_branch_nodes;

	IndepSetInst* 					inst;
	int								width;
	bool						    relax;

	int								layer;

	IS_Ordering*					ordering;			         /**< ordering */
	IS_Merging*						merger;						 /**< merging technique */

	/**
	 * Branch and bound attributes
	 */
	int								global_LB;
	int								global_UB;

	Bounds							branch_and_bound();

	//vector< vector<Node*> >		final_bdd;

	vector<int>						vertex_in_layer;
	int								final_width;
	int								cur_nodes_merged;

	vector<int>						selectable_vertices;

	// added for RL

	int eligible_vertex;
	Node* initial_node;
	NodeMap::iterator node_it;
	NodeMap::iterator existing_node_it;
	int current_vertex;
	Node* node;

	// auxiliaries
	int  generate_relaxation(IntSet &initial_state, int initial_longest_path);
	int  generate_restriction_with_ordering(IntSet &initial_state, int initial_longest_path);
	int  generate_restriction(IntSet &initial_state, int initial_longest_path);

	int  choose_next_vertex_min_size_next_layer();		 /**< for min state ordering */

	int  choose_next_vertex_min_size_next_layer_random(); /**< for random min state ordering */

	void relax_layer_shortestpath();
	void restrict_layer_shortestpath();

	void update_node_match(Node* nodeA, Node* nodeB);

	void initialize(IntSet &initial_state, int initial_longest_path);
	int generate_next_step_relaxation(int next_vertex);
	int generate_next_step_restriction(int next_vertex);
	int get_bound();

	IndepSetSolver(IndepSetInst* _inst, int _width);
};

inline IndepSetSolver::IndepSetSolver(IndepSetInst* _inst, int _width) : inst(_inst), width(_width)
{
	relax = false;

	in_state_counter = new int[inst->graph->n_vertices];
	active_vertex_map = new int[inst->graph->n_vertices];

	if( width != EXACT_BDD )
		nodes_layer.reserve(2*width*100);
	else {
		nodes_layer.reserve(100000000);
	}

	vertex_in_layer.resize(inst->graph->n_vertices+1);

	selectable_vertices.reserve(inst->graph->n_vertices);
}

inline void IndepSetSolver::update_node_match(Node* nodeA, Node* nodeB) {

	nodeA->longest_path = MAX(nodeA->longest_path, nodeB->longest_path);
	nodeB->longest_path = nodeA->longest_path;

}

inline void add_without_repetition(vector<Node*> &v, Node* node) {
	for( vector<Node*>::iterator it = v.begin(); it != v.end(); it++ ) {
		if( (*it) == node )
			return;
	}
	v.push_back(node);
}

/**
 * Merge nodes to meet maximum width.
 */
inline void IndepSetSolver::relax_layer_shortestpath() {

	sort(nodes_layer.begin(), nodes_layer.end(), CompareNodesLongestPath());

	IntSet* state = &(nodes_layer[width-1]->state);
	for( vector<Node*>::iterator node = nodes_layer.begin()+width; node != nodes_layer.end(); ++node) {
		state->union_with((*node)->state);

			delete (*node);
//		}
	}
	nodes_layer.resize(width);

	/**
	 * 2. Equivalence test on this new node
	 */
	Node* node;
	for( int i = 0; i <= width-2; i++ ) {

		node = nodes_layer[i];

		// check if this state already exists in layer nodes
		if( node->state.equals_to(*state) ) {

			// delete the last node
			delete nodes_layer.back();

			// remove it from queue
			nodes_layer.pop_back();

			break;
		}
	}

}

inline int IndepSetSolver::choose_next_vertex_min_size_next_layer() {

	int min = INF;
	int selected_vertex = -1;
	for( int i = 0; i < inst->graph->n_vertices; i++ ) {
		if( in_state_counter[i] > 0 && in_state_counter[i] < min ) {
			selected_vertex = i;
			min = in_state_counter[i];
		}
	}

	return selected_vertex;
}

inline int IndepSetSolver::choose_next_vertex_min_size_next_layer_random() {

	int selected_vertex = -1;

	RandomizedMinInState* min_state = (RandomizedMinInState*) ordering;

	// check if we will randomize our selection or not
	double probabilities[] = {min_state->prob, 1.0-min_state->prob};

	boost::random::discrete_distribution<> dist(probabilities);
	if( dist(min_state->gen) == 0 ) {

		// min state !
		int min = INF;

		for( int i = 0; i < inst->graph->n_vertices; i++ ) {
			if( in_state_counter[i] > 0 && in_state_counter[i] < min ) {
				selected_vertex = i;
				min = in_state_counter[i];
			}
		}

	} else {

		// randomized!
		selectable_vertices.clear();
		for( int v = 0; v < inst->graph->n_vertices; v++ ) {
			if( in_state_counter[v] > 0 ) {
				selectable_vertices.push_back(v);
			}
		}

		assert( selectable_vertices.size() > 0 );
		boost::random::uniform_int_distribution<> vertex_selector(0, (selectable_vertices.size()-1));

		return selectable_vertices[ vertex_selector(min_state->gen) ];
	}

	return selected_vertex;
}

#endif /* INDEPSET_SOLVER_HPP_ */


// Copyright 2006 Johan Rade
// Copyright 2011 Paul A. Bristow  To incorporate into Boost.Math
// Copyright 2012 Paul A. Bristow with new tests.

// 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)

#ifdef _MSC_VER
#  pragma warning(disable : 4127) // Expression is constant.
#endif

#define BOOST_TEST_MAIN

#include <boost/test/auto_unit_test.hpp>
#include <boost/math/special_functions/nonfinite_num_facets.hpp>
#include "s_.ipp" // To create test strings like std::basic_string<CharType> s = S_("0 -0");

#include <iomanip>
#include <locale>
#include <sstream>
#include <ostream>
#include <iostream>
#include <iomanip>
#include <limits>
#include <iostream>

namespace {

  // Using an anonymous namespace resolves ambiguities on platforms
  // with fpclassify etc functions at global scope.

  using namespace boost::math;
  using boost::math::signbit;
  using boost::math::changesign;
  using boost::math::isnan;

  //------------------------------------------------------------------------------

  template<class CharType, class ValType> void signed_zero_test_impl();
  // Loopback tests using all built-in char and floating-point types.

  BOOST_AUTO_TEST_CASE(signed_zero_test)
  {
    std::cout
      << "BuildInfo:" << '\n'
      << "  platform "  << BOOST_PLATFORM << '\n'
      << "  compiler "  << BOOST_COMPILER << '\n'
      << "  STL "       << BOOST_STDLIB << '\n'
      << "  Boost version " << BOOST_VERSION/100000     << "."
                            << BOOST_VERSION/100 % 1000 << "."
                            << BOOST_VERSION % 100
    << std::endl;

    signed_zero_test_impl<char, float>();
    signed_zero_test_impl<char, double>();
    signed_zero_test_impl<char, long double>();
    signed_zero_test_impl<wchar_t, float>();
    signed_zero_test_impl<wchar_t, double>();
    signed_zero_test_impl<wchar_t, long double>();
  }

  template<class CharType, class ValType> void signed_zero_test_impl()
  {

    if (signbit(static_cast<CharType>(-1e-6f) / (std::numeric_limits<CharType>::max)()) != -0)
    {
      BOOST_TEST_MESSAGE("Signed zero is not supported on this platform!");
      return;
    }

    std::locale old_locale;
    std::locale tmp_locale(
      old_locale, new nonfinite_num_put<CharType>(signed_zero));
    std::locale new_locale(tmp_locale, new nonfinite_num_get<CharType>);

    std::basic_stringstream<CharType> ss;
    ss.imbue(new_locale);

    std::basic_string<CharType> null = S_("");

    std::basic_string<CharType> s1 = S_("123");
    ss << s1 << std::endl;
    ss.str(null);

    BOOST_CHECK(ss.str() == null);  //

    ValType a1 = static_cast<ValType>(0); // zero.
    ValType a2 = (changesign)(static_cast<ValType>(0)); // negative signed zero.
    BOOST_CHECK(!(signbit)(a1)); //
    BOOST_CHECK((signbit)(a2));

    ss << a1 << ' ' << a2;

    std::basic_string<CharType> s = S_("0 -0"); // Expected.
    BOOST_CHECK(ss.str() == s);

    ValType b1, b2;
    ss >> b1 >> b2;  // Read back in.

    BOOST_CHECK(b1 == a1);
    BOOST_CHECK(b2 == a2);
    BOOST_CHECK(!(signbit)(b1));
    BOOST_CHECK((signbit)(b2));
    BOOST_CHECK(ss.rdstate() == std::ios_base::eofbit);
  }  //   template<class CharType, class ValType> void signed_zero_test_impl()

  // Checking output of types char using first default & then using signed_zero flag.
#define CHECKOUT(manips, expected)\
  {\
    {\
      std::locale old_locale;\
      std::locale tmp_locale(old_locale, new nonfinite_num_put<char>(0));\
      std::locale new_locale(tmp_locale, new nonfinite_num_get<char>);\
      std::ostringstream ss;\
      ss.imbue(new_locale);\
      ss << manips;\
      std::basic_string<char> s = S_(expected);\
      BOOST_CHECK_EQUAL(ss.str(), s);\
    }\
    {\
      std::locale old_locale;\
      std::locale tmp_locale(old_locale, new nonfinite_num_put<char>(signed_zero));\
      std::locale new_locale(tmp_locale, new nonfinite_num_get<char>);\
      std::ostringstream ss;\
      ss.imbue(new_locale);\
      ss << manips;\
      std::basic_string<char> s = S_(expected);\
      BOOST_CHECK_EQUAL(ss.str(), s);\
    }\
  }\

  BOOST_AUTO_TEST_CASE(misc_output_tests)
  { // Tests of output using a variety of output options.

     //
     // STD libraries don't all format zeros the same,
     // so figure out what the library-specific formatting is
     // and then make sure that our facet produces the same...
     //
     bool precision_after = false;  // Prints N digits after the point rather than N digits total
     bool triple_exponent = false;  // Has 3 digits in the exponent rather than 2.
     std::stringstream ss;
     ss << std::showpoint << std::setprecision(6) << 0.0;
     if(ss.str().size() == 8)
        precision_after = true;
     ss.str("");
     ss << std::scientific << 0.0;
     triple_exponent = ss.str().size() - ss.str().find_first_of('e') == 5;

    // Positive zero.
    CHECKOUT(0, "0"); // integer zero.
    CHECKOUT(0., "0"); // double zero.
    CHECKOUT(std::setw(2) << 0., " 0");
    CHECKOUT(std::setw(4) << 0., "   0");
    CHECKOUT(std::right << std::setw(4) << 0., "   0");
    CHECKOUT(std::left << std::setw(4) << 0., "0   ");
    CHECKOUT(std::setw(4) << std::setfill('*') << 0., "***0");
    CHECKOUT(std::setw(4) << std::internal << std::setfill('*') << 0., "***0"); // left adjust sign and right adjust value.
    CHECKOUT(std::showpos << std::setw(4) << std::internal << std::setfill('*') << 0., "+**0"); // left adjust sign and right adjust value.

   if(precision_after)
   {
// BOOST_STDLIB == ("Dinkumware standard library version" BOOST_STRINGIZE(_CPPLIB_VER)) )
    CHECKOUT(std::showpoint << 0., "0.000000"); //  std::setprecision(6)
    CHECKOUT(std::setprecision(2) << std::showpoint << 0., "0.00");
   }
   else
   {
    CHECKOUT(std::showpoint << 0., "0.00000"); //  std::setprecision(6)
    CHECKOUT(std::setprecision(2) << std::showpoint << 0., "0.0");
   }
    CHECKOUT(std::fixed << std::setw(5) << std::setfill('0') << std::setprecision(2) << 0., "00.00");
    CHECKOUT(std::fixed << std::setw(6) << std::setfill('0') << std::setprecision(2) << 0., "000.00");
    CHECKOUT(std::fixed << std::setw(6) << std::setfill('0') << std::setprecision(3) << 0., "00.000");
    CHECKOUT(std::fixed << std::setw(6) << std::setfill('*') << std::setprecision(3) << 0., "*0.000");
    CHECKOUT(std::fixed << std::setw(6) << std::setfill('*') << std::setprecision(2) << std::left << 0.0, "0.00**");

    CHECKOUT(std::showpos << 0., "+0");
    CHECKOUT(std::showpos << std::fixed << std::setw(6) << std::setfill('*') << std::setprecision(2) << std::left << 0.0, "+0.00*");
   if(triple_exponent)
   {
    CHECKOUT(std::scientific << std::showpoint << std::setw(10) << std::setfill('*') << std::setprecision(1) << std::left << 0., "0.0e+000**");
   }
   else
   {
    CHECKOUT(std::scientific << std::showpoint << std::setw(10) << std::setfill('*') << std::setprecision(1) << std::left << 0., "0.0e+00***");
   }
    CHECKOUT(std::fixed << std::showpoint << std::setw(6) << std::setfill('*') << std::setprecision(3) << std::left << 0., "0.000*");

    double nz = (changesign)(static_cast<double>(0)); // negative signed zero.
    CHECKOUT(nz, "-0");
    // CHECKOUT(std::defaultfloat << nz, "-0"); Only for C++11
    CHECKOUT(std::showpos << nz, "-0"); // Ignore showpos because is negative.
    CHECKOUT(std::setw(2) << nz, "-0");
    CHECKOUT(std::setw(4) << nz, "  -0");
    CHECKOUT(std::right << std::setw(4) << nz, "  -0");
    CHECKOUT(std::left << std::setw(4) << nz, "-0  ");
    CHECKOUT(std::setw(4) << std::setfill('*') << nz, "**-0");
    CHECKOUT(std::setw(4) << std::internal << std::setfill('*') << nz, "-**0"); // Use std::internal to left adjust sign and right adjust value.
    CHECKOUT(std::showpos << std::setw(4) << std::internal << std::setfill('*') << nz, "-**0");

    CHECKOUT(std::fixed << std::setw(5) << std::setfill('0') << std::setprecision(2) << 0., "00.00");
    CHECKOUT(std::fixed << std::setw(6) << std::setfill('0') << std::setprecision(2) << 0., "000.00");
    CHECKOUT(std::fixed << std::setw(6) << std::setfill('0') << std::setprecision(3) << 0., "00.000");
    CHECKOUT(std::fixed << std::setw(6) << std::setfill('*') << std::setprecision(3) << 0., "*0.000");
    CHECKOUT(std::fixed << std::setw(6) << std::setfill('*') << std::setprecision(2) << std::left << 0.0, "0.00**");
    CHECKOUT(std::setprecision(2) << nz, "-0"); // No showpoint, so no decimal point nor trailing zeros.
   if(precision_after)
   {
    CHECKOUT(std::setprecision(2) << std::showpoint << nz, "-0.00"); // or "-0.0"
    CHECKOUT(std::setw(1) << std::setprecision(3) << std::showpoint << nz, "-0.000"); // Not enough width for precision overflows width.  or "-0.00"
   }
   else
   {
    CHECKOUT(std::setprecision(2) << std::showpoint << nz, "-0.0"); // or "-0.00"
    CHECKOUT(std::setw(1) << std::setprecision(3) << std::showpoint << nz, "-0.00"); // Not enough width for precision overflows width.  or "-0.000"
   }
   if(triple_exponent)
   {
    CHECKOUT(std::scientific << std::showpoint << std::setw(10) << std::setfill('*') << std::setprecision(1) << std::left << nz, "-0.0e+000*"); // -0.0e+00**
   }
   else
   {
    CHECKOUT(std::scientific << std::showpoint << std::setw(10) << std::setfill('*') << std::setprecision(1) << std::left << nz, "-0.0e+00**"); // -0.0e+000*
   }
    CHECKOUT(std::fixed << std::showpoint << std::setw(6) << std::setfill('*') << std::setprecision(3) << std::left << 0., "0.000*");

    // Non zero values.

    CHECKOUT(std::showpos << std::fixed << std::setw(6) << std::setfill('*') << std::setprecision(2) << std::left << 42., "+42.00");
    CHECKOUT(std::showpos << std::fixed << std::setw(6) << std::setfill('*') << std::setprecision(2) << std::left << 4.2, "+4.20*");
    CHECKOUT(std::showpos << std::fixed << std::setw(6) << std::setfill('*') << std::setprecision(2) << std::left << 1.22, "+1.22*");
    CHECKOUT(std::showpos << std::fixed << std::setw(6) << std::setfill('*') << std::setprecision(2) << std::left << 0.12, "+0.12*");

    CHECKOUT(std::setprecision(4) << std::showpoint << 1.2, "1.200");

  }

}   // anonymous namespace

/*

Output:

test_signed_zero.cpp
  Running 2 test cases...
  Platform: Win32
  Compiler: Microsoft Visual C++ version 10.0
  STL     : Dinkumware standard library version 520
  Boost   : 1.49.0
  Entering test suite "Master Test Suite"
  Entering test case "signed_zero_test"
  Leaving test case "signed_zero_test"; testing time: 2ms
  Entering test case "misc_output_tests"
  Leaving test case "misc_output_tests"; testing time: 15ms
  Leaving test suite "Master Test Suite"

  *** No errors detected

*/


#include <string>
#include <sstream>
#include <iostream>
#include <map>
#include <regex>
#include <fstream>
#include <boost/algorithm/string.hpp>

#include "../common.hpp"

using namespace std;

map<struct xy, int> trace(string wirepath) {
    int x = 0;
    int y = 0;
    int length = 0;

    map<struct xy, int> panel;

    string instruction;
    stringstream ss (wirepath);
    while (getline(ss, instruction, ',')) {
        assert(instruction.size() >= 2);
        char direction = instruction[0];
        int count = stoi(instruction.substr(1));

        int dx = 0;
        int dy = 0;
        switch (direction) {
            case 'L':
                dx = -1;
                break;
            case 'R':
                dx = 1;
                break;
            case 'U':
                dy = -1;
                break;
            case 'D':
                dy = 1;
                break;

            default:
                assert(false);
        }

        for (int i = 0; i < count; i++) {
            panel[make_xy(x, y)] = length;
            x += dx;
            y += dy;
            length++;
        }
    }

    return panel;
}

// Returns the bounds for this map.
// TODO Turns out this wasn't needed :(
struct bounds bounds (map<struct xy, int> m) {
    struct bounds b;

    assert(m.size() > 0);
    b.min_x = b.max_x = m.begin()->first.x;
    b.min_y = b.max_y = m.begin()->first.y;

    for(auto const& [key, val] : m) {
        b.min_x = min(b.min_x, key.x);
        b.max_x = max(b.max_x, key.x);
        b.min_y = min(b.min_y, key.y);
        b.max_y = max(b.max_y, key.y);
    }

    return b;
}

int main() {
    ifstream file ("2019/3.txt");
    if (!file.is_open()) {
        cout << "Failed to open file: " << strerror(errno) << endl;
        return -1;
    }

    vector<string> wires;
    string line;
    while (getline(file, line, '\n')) {
        boost::trim(line);

        if (line == "") {
            continue;
        }
        wires.push_back(line);
    }
    assert(wires.size() == 2);

    const auto w1 = trace(wires[0]);
    const auto w2 = trace(wires[1]);

    int min_distance = INT_MAX;
    int min_length = INT_MAX;

    // Find where they cross
    for(auto const& [key, val] : w1) {
        if (w2.find(key) != w2.end()) {
            // Skip central port
            if (key.x == 0 && key.y == 0) {
                continue;
            }
            // Cross at key
            min_distance = min(min_distance, manhattan(make_xy(0,0), key));
            min_length = min(min_length, val + w2.at(key));
        }
    }

    cout << "Answer 3.1: " << min_distance << endl;
    cout << "Answer 3.2: " << min_length << endl;

    file.close();
}


// MIT License

// Copyright (c) 2021 Daniel Kowalczyk

// 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.

#include "BranchNode.hpp"
#include <fmt/core.h>
#include <algorithm>
#include <array>
#include <boost/multiprecision/cpp_int.hpp>
#include <cmath>
#include <cstddef>
#include <memory>
#include <numeric>
#include <range/v3/all.hpp>
#include <vector>
#include "DebugLvl.hpp"
#include "NodeData.h"
#include "Parms.h"
#include "PricerSolverBase.hpp"
#include "Statistics.h"
#include "branch-and-bound/btree.h"
#include "or-utils/util.h"

namespace fmt {
template <>
struct formatter<boost::multiprecision::cpp_int> {
    template <typename ParseContext>
    constexpr auto parse(ParseContext& ctx) {
        return ctx.begin();
    }

    template <typename FormatContext>
    auto format([[maybe_unused]] const boost::multiprecision::cpp_int& p,
                FormatContext&                                         ctx) {
        return format_to(ctx.begin(), "boost::number");
    }
};
}  // namespace fmt

BranchNodeBase::BranchNodeBase(std::unique_ptr<NodeData> _pd, bool _isRoot)
    : State(_isRoot),
      pd(std::move(_pd)) {
    if (_isRoot) {
        set_ub(double(pd->opt_sol.tw));
        pd->stat.start_resume_timer(Statistics::lb_root_timer);
        pd->build_rmp();
        pd->solve_relaxation();
        pd->compute_lower_bound();
        pd->stat.suspend_timer(Statistics::lb_root_timer);
        set_lb(pd->lower_bound);
        if (pd->solver->get_is_integer_solution()) {
            set_obj_value(pd->LP_lower_bound);
        }
    }
}

void BranchNodeBase::branch(BTree* bt) {
    auto*       solver = get_pricersolver();
    const auto& instance = get_instance_info();
    const auto& parms = pd->parms;

    if (!parms.strong_branching.value() && debug_lvl(1)) {
        fmt::print("\nDOING STRONG BRANCHING...\n\n");
    }

    if (bt->getGlobalUB() < pd->upper_bound) {
        pd->upper_bound = static_cast<int>(bt->getGlobalUB());
        solver->UB = bt->getGlobalUB();
    }

    std::array<BranchCandidate, NbCandidates> candidates{};
    ranges::make_heap(candidates, std::greater<>{},
                      [](const auto& tmp) { return tmp.score; });

    auto tmp_t = 0;
    auto x_ref = solver->calculate_job_time();
    auto rng_zip = ranges::views::zip(x_ref, instance.jobs);

    /**
     * @brief
     * First we deduce branching based on SOS where the coefficients are
     * associated to the objective function. The objective functions here are
     * regular objective. Are always applicable  if the objective functions are
     * regular.
     */
    auto rng_sum_wt = ranges::views::filter([&tmp_t,
                                             &instance](const auto& tmp_aux) {
        const auto& [x, j] = tmp_aux;
        auto sum = ranges::accumulate(x, 0.0, [&j](auto a, auto b) {
            return (j->weighted_tardiness_start(b.get_t()) * b.get_value() + a);
        });

        if (safe_frac(sum) < EPS) {
            return false;
        }

        auto aux_vec = ranges::views::iota(int{}, instance.H_max + 1);
        auto lb_it = ranges::upper_bound(
            aux_vec, sum, std::less<>(),
            [&j](auto t) { return j->weighted_tardiness_start(t); });

        tmp_t = (*lb_it) - 1;
        return true;
    });

    for (auto&& [x_j, job] :
         rng_zip | rng_sum_wt |
             ranges::views::take(std::min(size_t{50}, instance.nb_jobs))) {
        auto aux = BranchCandidate(pd->get_score_value(),
                                   pd->create_child_nodes(job->job, tmp_t));
        ranges::make_heap(candidates, std::greater<>{},
                          [](const auto& tmp) { return tmp.score; });
        if (aux.score > candidates.front().score) {
            std::swap(aux, candidates.front());
        }
    }

    /**
     * @brief If the previous generator does not construct any branching
     * candidates even if the solutions are not integer. We apply different
     * branching scheme based of SOS. Here we look a the ending time.
     */
    if (ranges::all_of(candidates, [](const auto& tmp) { return tmp.empty; })) {
        auto rng_t = ranges::views::filter([&tmp_t,
                                            &instance](const auto& tmp_aux) {
            auto&& [x, j] = tmp_aux;
            auto sum = ranges::accumulate(x, 0.0, [&j](auto a, auto b) {
                return ((j->processing_time + b.get_t()) * b.get_value() + a);
            });

            if(safe_frac(sum) < EPS){
                return false;
            }

            auto aux_vec = ranges::views::iota(int{}, instance.H_max + 1);
            auto lb_it = ranges::upper_bound(
                aux_vec, sum, std::less<>(),
                [&j](auto t) { return (j->processing_time + t); });

            tmp_t = (*lb_it) - 1;
            return true;
        });

        for (auto&& [x_j, job] :
             rng_zip | rng_t |
                 ranges::views::take(std::min(size_t{50}, instance.nb_jobs))) {
            auto aux = BranchCandidate(pd->get_score_value(),
                                       pd->create_child_nodes(job->job, tmp_t));
            ranges::make_heap(candidates, std::greater<>{},
                              [](const auto& tmp) { return tmp.score; });
            if (aux.score > candidates.front().score) {
                std::swap(aux, candidates.front());
            }
        }
    }

    /**
     * @brief Here we branch on the cumulative variable z_jt = sum_{t' <= t}
     * x_jt. We choose the z_jt closest to 0.5.
     */
    if (ranges::all_of(candidates, [](const auto& tmp) { return tmp.empty; })) {
        auto rng_cum = ranges::views::filter([&tmp_t](const auto& tmp_aux) {
            auto&& [x_j, job] = tmp_aux;

            auto br_point_it = ranges::max(x_j, std::less<>{}, [](auto& x) {
                return 0.5 - std::abs(x.get_cum_value() -
                                      std::floor(x.get_cum_value()) - 0.5);
            });
            auto cum_aux = br_point_it.get_cum_value();
            auto aux =  safe_frac(cum_aux);

            tmp_t = static_cast<int>(br_point_it.get_t());
            return (aux > EPS);
        });

        for (auto&& [x_j, job] : rng_zip | rng_cum) {
            auto aux_data = BranchCandidate(
                pd->get_score_value(), pd->create_child_nodes(job->job, tmp_t));
            ranges::make_heap(candidates, std::greater<>{},
                              [](const auto& tmp) { return tmp.score; });
            if (aux_data.score > candidates.front().score) {
                std::swap(aux_data, candidates.front());
            }
        }
    }

    if (ranges::all_of(candidates, [](const auto& tmp) { return tmp.empty; })) {
        fmt::print(stderr, "ERROR: no branching found!\n");
        for (auto&& [it, job] : rng_zip) {
            fmt::print(stderr, "j={}:", job->job);
            for (auto& x : it) {
                fmt::print(stderr, " ({},{})", x.get_t(), x.get_value());
            }
            fmt::print(stderr, "\n");
        }
        exit(-1);
    }

    auto best_min_gain = std::numeric_limits<double>::min();
    auto best_job = -1;
    auto best_time = 0;

    std::array<std::unique_ptr<BranchNodeBase>, 2> best{};
    auto                                           nb_cand = (pd->depth <= 7)
                                                                 ? std::min(std::max(parms.strong_branching.value(), 1),
                                                                            static_cast<int>(candidates.size()))
                                                                 : 1;

    candidates |= ranges::actions::sort(
        std::greater<>{}, [](const auto& tmp) { return tmp.score; });

    auto rng_best_cand =
        candidates |
        ranges::views::filter([](const auto& tmp) { return !(tmp.empty); });

    auto nb_non_improvements = 0UL;

    /**
     * @brief Adding nodes to branch&bound tree.
     */
    for (auto& it : rng_best_cand | ranges::views::take(nb_cand)) {
        std::array<std::unique_ptr<BranchNodeBase>, 2> child_nodes;
        std::array<double, 2>                          scores{};
        std::array<bool, 2>                            fathom{};
        auto                                           left = false;

        for (auto&& [data, node, score, f] : ranges::views::zip(
                 it.data_child_nodes, child_nodes, scores, fathom)) {
            node = std::make_unique<BranchNodeBase>(std::move(data));
            node->compute_bounds(bt);

            auto cost = node->pd->LP_lower_bound;
            if (debug_lvl(0)) {
                fmt::print(
                    "STRONG BRANCHING {} PROBE: j = {}, t = {},"
                    " DWM LB = {:9.2f} in iterations {} \n\n",
                    left ? "LEFT" : "RIGHT", node->pd->branch_job,
                    node->pd->completiontime, cost + pd->instance.off,
                    node->pd->iterations);
            }
            if (cost >= bt->getGlobalUB() - 1.0 + IntegerTolerance ||
                node->get_data_ptr()->solver->get_is_integer_solution()) {
                f = true;
            }
            score = (parms.scoring_parameter.value() ==
                     weighted_sum_scoring_parameter)
                        ? cost / pd->LP_lower_bound
                        : std::abs(cost - pd->LP_lower_bound);
            left = true;
        }

        auto best_score = parms.scoring_function(scores);

        if (best_score > best_min_gain ||
            ranges::any_of(fathom, ranges::identity{})) {
            best_min_gain = best_score;
            best = std::move(child_nodes);
            nb_non_improvements /= 2;
        } else {
            ++nb_non_improvements;
        }

        if (ranges::any_of(fathom, std::identity{}) ||
            nb_non_improvements > 3) {
            break;
        }
    }

    /** Process the branching nodes insert them in the tree */
    bt->set_state_bounds_computed(true);
    for (auto& it : best) {
        bt->process_state(std::move(it));
    }
    bt->set_state_bounds_computed(false);
    bt->update_global_lb();

    if (debug_lvl(1)) {
        fmt::print("Branching choice: j = {}, t = {}, best_gain = {}\n",
                   best_job, best_time, best_min_gain + pd->instance.off);
    }
}

void BranchNodeBase::compute_bounds([[maybe_unused]] BTree* bt) {
    if (!pd->solver->structure_feasible()) {
        pd->status = NodeData::infeasible;
        pd->LP_lower_bound_dual = pd->LP_lower_bound = pd->LP_lower_bound_BB =
            pd->upper_bound;
        pd->lower_bound = pd->upper_bound;
        return;
    }
    pd->compute_lower_bound();
    set_lb(pd->lower_bound);
    if (pd->solver->get_is_integer_solution()) {
        set_obj_value(pd->lower_bound);
    }
}

void BranchNodeBase::assess_dominance([[maybe_unused]] State* otherState) {}

bool BranchNodeBase::is_terminal_state() {
    auto* solver = pd->solver.get();
    set_feasible(pd->status != NodeData::infeasible);
    return (solver->get_is_integer_solution() || !is_feasible());
}

void BranchNodeBase::apply_final_pruning_tests([[maybe_unused]] BTree* bt) {
    auto* pricing_solver = get_pricersolver();
    // auto* pd_ptr = get_data_ptr();
    if (pricing_solver->print_num_paths() < MAX_NB_PATHS) {
        pricing_solver->project_sol_on_original_variables(pd->opt_sol);
        pricing_solver->build_mip();
        auto feasible_aux = pricing_solver->evaluate_mip_model();
        set_dominated(!feasible_aux);
    }
}

void BranchNodeBase::update_data(double upper_bound) {
    // auto* statistics = pd->stat;
    pd->stat.global_upper_bound = static_cast<int>(upper_bound);
}

void BranchNodeBase::print(const BTree* bt) const {
    auto* solver = pd->solver.get();
    if (get_is_root_node()) {
        fmt::print("{0:^10}|{1:^75}|{2:^30}|{3:^10}|{4:^10}|\n", "Nodes",
                   "Current Node", "Objective Bounds", "Branch", "Work");
        fmt::print(
            R"({0:^5}{1:^5}|{2:>10}{3:>10}{10:>10}{12:>45}|{4:>10}{5:>10}{6:>10}|{7:>5}{8:>5}|{9:>5}{11:>5}|
)",
            "Expl", "UnEx", "Obj", "Depth", "Primal", "Dual", "Gap", "Job",
            "Time", "Time", "Size", "Iter", "NB Paths");
    }
    fmt::print(
        R"({0:>5}{1:>5}|{2:10.2f}{3:>10}{4:>10}{12:>45}|{5:10.2f}{6:10.2f}{7:10.2f}|{8:>5}{9:>5}|{10:>5}{11:>5}|
)",
        bt->get_nb_nodes_explored(), bt->get_nb_nodes(),
        pd->LP_lower_bound + pd->instance.off, pd->depth,
        solver->get_nb_vertices(), bt->getGlobalUB() + pd->instance.off,
        bt->getGlobalLB() + pd->instance.off, 0.0, pd->branch_job,
        pd->completiontime, bt->get_run_time_start(), pd->iterations,
        solver->print_num_paths().str());
}

BranchCandidate::BranchCandidate(
    double                                     _score,
    std::array<std::unique_ptr<NodeData>, 2>&& child_nodes)
    : score(_score),
      empty(false),
      data_child_nodes(std::move(child_nodes)) {
    const auto&           parms = data_child_nodes[0]->parms;
    std::array<double, 2> scores{};

    for (auto&& [node, s] : ranges::views::zip(data_child_nodes, scores)) {
        s = (parms.scoring_parameter.value() == weighted_sum_scoring_parameter)
                ? _score / node->get_score_value()
                : std::abs(_score - node->get_score_value());
    }
    score = parms.scoring_function(scores);
}

BranchCandidate::BranchCandidate(double _score) : score(_score) {}

BranchNodeRelBranching::BranchNodeRelBranching(std::unique_ptr<NodeData> _data,
                                               bool                      isRoot)
    : BranchNodeBase(std::move(_data), isRoot) {}

void BranchNodeRelBranching::branch(BTree* bt) {
    auto* node_data = get_data_ptr();
    auto* pricing_solver = get_pricersolver();
    // auto&       parms = node_data->parms;
    const auto& instance = get_instance_info();
    // const auto  Tmax = instance.H_max;

    if (bt->getGlobalUB() < node_data->upper_bound) {
        node_data->upper_bound = static_cast<int>(bt->getGlobalUB());
        pricing_solver->UB = bt->getGlobalUB();
    }

    std::array<BranchCandidate, NbCandidates> candidates{};
    // auto& x_job_time = pricing_solver->calculate_job_time();
    // auto& aux_branch_history = bt->get_branch_history();

    // for (auto& it : aux_branch_history) {
    //     auto j = it.first / (Tmax + 1);
    //     auto t = it.first % (Tmax + 1);
    //     auto aux_x = x_ref.second[j][t];
    //     if (std::min(aux_x - std::floor(aux_x), std::ceil(aux_x) - aux_x) >
    //         EPS) {
    //         auto aux = BranchCandidate(it.second.get_score());
    //         ranges::make_heap(candidates, std::greater<>{},
    //                           [](const auto& tmp) { return tmp.score; });
    //         if (aux.score > candidates.front().score) {
    //             std::swap(aux, candidates.front());
    //         }
    //     }
    // }

    // for (auto&& [z_j, job] : ranges::views::zip(x_ref.second, instance.jobs))
    // {
    //     for (auto&& [t, x_j_t] : z_j | ranges::views::enumerate) {
    //         auto left_gain = x_j_t - std::floor(x_j_t);
    //         auto right_gain = std::ceil(x_j_t) - x_j_t;
    //         if (std::min(left_gain, right_gain) > EPS) {
    //             auto aux_it =
    //                 aux_branch_history.find(job->job * instance.H_max + t);
    //             if (aux_it != aux_branch_history.end()) {
    //                 // candidates.emplace_back(
    //                 // aux_it->second.compute_score(parms.scoring_function,
    //                 //                                  left_gain,
    //                 right_gain),
    //                 //     node_data->create_child_nodes(job->job, t));
    //             }
    //         }
    //     }
    // }

    // for (auto&& [x_j, job] : ranges::views::zip(x_job_time, instance.jobs)) {
    //     auto aux_vec = ranges::views::iota(size_t{}, x_j.size()) |
    //                    ranges::views::transform([&](const auto& tmp) {
    //                        return job->weighted_tardiness_start(tmp);
    //                    }) |
    //                    ranges::to_vector;
    //     auto sum = ranges::inner_product(x_j, aux_vec, 0.0);
    //     auto lb_it = ranges::lower_bound(aux_vec, sum);
    //     if (*lb_it - sum > EPS &&
    //         std::min(sum - std::floor(sum), std::ceil(sum) - sum) > EPS) {
    //         // auto tmp_t = ranges::distance(aux_vec.begin(), lb_it) - 1;
    //         // candidates.emplace_back(
    //         //     node_data->LP_lower_bound,
    //         //     node_data->create_child_nodes(job->job, tmp_t));
    //     }
    // }

    // if (candidates.empty()) {
    //     for (auto&& [x_j, job] :
    //          ranges::views::zip(x_job_time, instance.jobs)) {
    //         auto aux_vec =
    //             ranges::views::iota(0UL, x_j.size()) |
    //             ranges::views::transform([&](const auto& tmp) { return tmp;
    //             }) | ranges::to_vector;
    //         auto sum = ranges::inner_product(x_j, aux_vec, 0.0);
    //         auto lb_it = ranges::lower_bound(aux_vec, sum);
    //         if (*lb_it - sum > EPS &&
    //             std::min(sum - std::floor(sum), std::ceil(sum) - sum) > EPS)
    //             {
    //             // auto tmp_t = ranges::distance(aux_vec.begin(), lb_it) - 1;
    //             // candidates.emplace_back(
    //             //     node_data->LP_lower_bound,
    //             //     node_data->create_child_nodes(job->job, tmp_t));
    //         }
    //     }
    // }

    // if (candidates.empty()) {
    //     for (auto&& [x_j, job] :
    //          ranges::views::zip(x_job_time, instance.jobs)) {
    //         auto part_sum = ranges::views::partial_sum(x_j, std::plus<>{});
    //         auto br_point_it =
    //             ranges::lower_bound(part_sum, parms.branching_point.value() -
    //             EPS);
    //         auto aux = std::min(*br_point_it - std::floor(*br_point_it),
    //                             std::ceil(*br_point_it) - *br_point_it);
    //         if (aux > EPS) {
    //             // auto tmp_t = ranges::distance(part_sum.begin(),
    //             br_point_it);
    //             // candidates.emplace_back(
    //             //     node_data->LP_lower_bound,
    //             //     node_data->create_child_nodes(job->job, tmp_t));
    //         }
    //     }
    // }

    // if (candidates.empty()) {
    //     for (auto&& [x_j, job] :
    //          ranges::views::zip(x_job_time, instance.jobs)) {
    //         auto part_sum = ranges::views::partial_sum(x_j, std::plus<>{});
    //         auto br_point_it = ranges::upper_bound(part_sum, 0.0);
    //         auto aux = std::min(*br_point_it - std::floor(*br_point_it),
    //                             std::ceil(*br_point_it) - *br_point_it);
    //         if (aux > EPS) {
    //             // auto tmp_t = ranges::distance(part_sum.begin(),
    //             br_point_it);
    //             // candidates.emplace_back(
    //             //     node_data->LP_lower_bound,
    //             //     node_data->create_child_nodes(job->job, tmp_t));
    //         }
    //     }
    // }

    candidates |=
        ranges::actions::sort(std::greater<>{}, &BranchCandidate::score);

    // for (auto& it : candidates) {
    // }

    std::array<std::unique_ptr<BranchNodeRelBranching>, 2> best = {nullptr,
                                                                   nullptr};
    // std::unique_ptr<BranchNodeRelBranching>                best_left =
    // nullptr;

    auto best_min_gain = 0.0;
    auto best_job = -1;
    auto best_time = 0;

    bt->set_state_bounds_computed(true);
    for (auto& it : best) {
        bt->process_state(std::move(it));
    }
    // bt->process_state(std::move(best_right));
    // bt->process_state(std::move(best_left));
    bt->set_state_bounds_computed(false);
    bt->update_global_lb();

    if (debug_lvl(1)) {
        fmt::print("Branching choice: j = {}, t = {}, best_gain = {}\n",
                   best_job, best_time, best_min_gain + instance.off);
    }
}

const Instance& BranchNodeBase::get_instance_info() const {
    return pd->instance;
}

PricerSolverBase* BranchNodeBase::get_pricersolver() const {
    return pd->solver.get();
}