#include <boost/test/unit_test.hpp>

#include "logs.h"
#include "route.h"

using namespace GPS;

/*
Test suite to check function route::totalHeightGain function correctly,
documentation can be found in the form of comments above each test case.
*/
BOOST_AUTO_TEST_SUITE( Route_totalHeightGain_N0720387 )

const bool isFileName = true;

/*
Test case checks that an incline with all negative values is calculated correctly,
testing that subtraction is done properly with all negative values.
*/
BOOST_AUTO_TEST_CASE( inclineBelowSea )
{
   Route route = Route(LogFiles::GPXRoutesDir + "N0720387-inclineBelowSeaLevel.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.totalHeightGain(), 100 );
}

/*
Test case checks that an incline across the sea level boundary (negative
elevation to positive) is calculated correctly testing that subtraction
is done properly with changing signs.
*/
BOOST_AUTO_TEST_CASE( inclineAcrossSea )
{
   Route route = Route(LogFiles::GPXRoutesDir + "N0720387-inclineAcrossSeaLevel.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.totalHeightGain(), 100 );
}

/*
Test case checks that a decline across the sea level boundary (positive
elevation to negative) is calculated correctly testing that subtraction
is done properly with changing signs and that declining height differences
do not register as height increases.
*/
BOOST_AUTO_TEST_CASE( declineAcrossSea )
{
   Route route = Route(LogFiles::GPXRoutesDir + "N0720387-declineAcrossSeaLevel.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.totalHeightGain(), 0 );
}

/*
Test case checks that a decline followed by an incline only counts the
positive elevation increases and though net elevation is 0, total increase
is above 0.
*/
BOOST_AUTO_TEST_CASE( declineIncline )
{
   Route route = Route(LogFiles::GPXRoutesDir + "N0720387-declineIncline.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.totalHeightGain(), 100 );
}

/*
Test case checks that an incline followed by a decline counts the positive
elevation changes and the negatives that follow are ignored and do not effect
the total height gain.
*/
BOOST_AUTO_TEST_CASE( inclineDecline )
{
   Route route = Route(LogFiles::GPXRoutesDir + "N0720387-inclineDecline.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.totalHeightGain(), 100 );
}

/*
Test case checks that lots of big inclines and declines following eachother, going over
and under the sea level is correctly calculated.
*/
BOOST_AUTO_TEST_CASE( changesLotsAcrossSea )
{
   Route route = Route(LogFiles::GPXRoutesDir + "N0720387-changesAcrossSeaLevel.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.totalHeightGain(), 60000 );
}

/*
Test case checks that a log file with one position functions correctly and does not attempt
to access a second element of the vector, causing memory access violation. The route with
one point does not gain height and thus total height gained should be 0.
*/
BOOST_AUTO_TEST_CASE( onePosition )
{
   Route route = Route(LogFiles::GPXRoutesDir + "N0720387-onePosition.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.totalHeightGain(), 0 );
}

/*
Test case checks that a log file with huge increases in elevation are stored correctly,
although this may be an unrealistic log file, it assures that long routes can store
their total effectively.
*/
BOOST_AUTO_TEST_CASE( largeIncrease )
{
   Route route = Route(LogFiles::GPXRoutesDir + "N0720387-largeIncrease.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.totalHeightGain(), 26000000000 );
}

/*
Test case checks that a log file with tiny increases in elevation are correctly picked up
and tracked, although this may be too precise of a precision compared to a real life
instrument that creates GPX log files, this test proves that the function can handle tiny
increases and is not limited by data types.
*/
BOOST_AUTO_TEST_CASE( smallIncrease )
{
   Route route = Route(LogFiles::GPXRoutesDir + "N0720387-smallIncrease.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.totalHeightGain(), 0.00026, 0.0000000001 );
}

BOOST_AUTO_TEST_SUITE_END()


// This file is part of the dune-gdt project:
//   https://github.com/dune-community/dune-gdt
// Copyright 2010-2018 dune-gdt developers and contributors. All rights reserved.
// License: Dual licensed as BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//      or  GPL-2.0+ (http://opensource.org/licenses/gpl-license)
//          with "runtime exception" (http://www.dune-project.org/license.html)
// Authors:
//   Rene Milk      (2017 - 2018)
//   Tobias Leibner (2017)

#ifndef DUNE_GDT_OPERATORS_FV_RECONSTRUCTION_LINEAR_HH
#define DUNE_GDT_OPERATORS_FV_RECONSTRUCTION_LINEAR_HH

#include <boost/multi_array.hpp>

#include <dune/geometry/quadraturerules.hh>

#include <dune/xt/common/fvector.hh>
#include <dune/xt/common/lapacke.hh>
#include <dune/xt/common/parallel/threadstorage.hh>
#include <dune/xt/common/parameter.hh>

#include <dune/xt/grid/walker.hh>

#include <dune/xt/la/algorithms/qr.hh>
#include <dune/xt/la/eigen-solver.hh>

#include <dune/gdt/operators/interfaces.hh>

#include "../quadrature.hh"
#include "reconstructed_function.hh"
#include "slopelimiters.hh"

namespace Dune {
namespace GDT {
namespace internal {

template <class AnalyticalFluxType, class MatrixImp, class VectorImp>
class JacobianWrapperBase
{
public:
  using MatrixType = MatrixImp;
  using VectorType = VectorImp;
  static constexpr size_t dimDomain = AnalyticalFluxType::dimDomain;
  static constexpr size_t dimRange = AnalyticalFluxType::dimRange;
  using DomainType = typename AnalyticalFluxType::DomainType;
  using RangeFieldType = typename AnalyticalFluxType::RangeFieldType;
  using EntityType = typename AnalyticalFluxType::EntityType;
  using JacobianType = FieldVector<MatrixType, dimDomain>;
  using StateRangeType = typename AnalyticalFluxType::StateRangeType;

  JacobianWrapperBase()
    : computed_(false)
  {
  }

  virtual ~JacobianWrapperBase()
  {
  }

  virtual void get_jacobian(const size_t dd,
                            const EntityType& entity,
                            const AnalyticalFluxType& analytical_flux,
                            const DomainType& x_in_inside_coords,
                            const StateRangeType& u,
                            const XT::Common::Parameter& param) = 0;

  virtual void get_jacobian(const EntityType& entity,
                            const AnalyticalFluxType& analytical_flux,
                            const DomainType& x_in_inside_coords,
                            const StateRangeType& u,
                            const XT::Common::Parameter& param) = 0;

  virtual void compute(const size_t dd) = 0;

  virtual void compute()
  {
    for (size_t dd = 0; dd < dimDomain; ++dd)
      compute(dd);
  }

  virtual bool computed(const size_t dd) const
  {
    return computed_[dd];
  }

  virtual bool computed() const
  {
    for (size_t dd = 0; dd < dimDomain; ++dd)
      if (!computed(dd))
        return false;
    return true;
  }

  virtual std::unique_ptr<JacobianType>& jacobian()
  {
    return jacobian_;
  }

  virtual const std::unique_ptr<JacobianType>& jacobian() const
  {
    return jacobian_;
  }

  virtual MatrixType& jacobian(const size_t dd)
  {
    return (*jacobian_)[dd];
  }

  virtual const MatrixType& jacobian(const size_t dd) const
  {
    return (*jacobian_)[dd];
  }

  virtual void apply_eigenvectors(const size_t dd, const VectorType& x, VectorType& ret) const = 0;

  virtual void apply_inverse_eigenvectors(const size_t dd, const VectorType& x, VectorType& ret) const = 0;

protected:
  template <class MatImp>
  static XT::Common::Configuration create_eigensolver_opts()
  {
    using EigenSolverOptionsType = typename XT::LA::EigenSolverOptions<MatImp>;
    using MatrixInverterOptionsType = typename XT::LA::MatrixInverterOptions<MatImp>;
    XT::Common::Configuration eigensolver_options = EigenSolverOptionsType::options(EigenSolverOptionsType::types()[0]);
    //    XT::Common::Configuration eigensolver_options = EigenSolverOptionsType::options("shifted_qr");
    eigensolver_options["assert_eigendecomposition"] = "1e-6";
    eigensolver_options["assert_real_eigendecomposition"] = "1e-6";
    eigensolver_options["disable_checks"] =
#ifdef NDEBUG
        "true";
#else
        "false";
#endif
    XT::Common::Configuration matrix_inverter_options = MatrixInverterOptionsType::options();
    matrix_inverter_options["post_check_is_left_inverse"] = "1e-6";
    matrix_inverter_options["post_check_is_right_inverse"] = "1e-6";
    eigensolver_options.add(matrix_inverter_options, "matrix-inverter");
    return eigensolver_options;
  } // ... create_eigensolver_opts()

  std::unique_ptr<JacobianType> jacobian_;
  FieldVector<bool, dimDomain> computed_;
}; // class JacobianWrapperBase<...>

template <class AnalyticalFluxType,
          class MatrixType = FieldMatrix<typename AnalyticalFluxType::RangeFieldType,
                                         AnalyticalFluxType::dimRange,
                                         AnalyticalFluxType::dimRange>,
          class VectorType = FieldVector<typename AnalyticalFluxType::RangeFieldType, AnalyticalFluxType::dimRange>>
class JacobianWrapper : public JacobianWrapperBase<AnalyticalFluxType, MatrixType, VectorType>
{
  using BaseType = JacobianWrapperBase<AnalyticalFluxType, MatrixType, VectorType>;

protected:
  using V = XT::Common::VectorAbstraction<VectorType>;
  using M = XT::Common::MatrixAbstraction<MatrixType>;
  using EigenSolverType = typename XT::LA::EigenSolver<MatrixType>;
  using typename BaseType::RangeFieldType;

public:
  static constexpr size_t dimDomain = AnalyticalFluxType::dimDomain;
  static constexpr size_t dimRange = AnalyticalFluxType::dimRange;
  using typename BaseType::JacobianType;
  using typename BaseType::EntityType;
  using typename BaseType::DomainType;
  using typename BaseType::StateRangeType;

  using BaseType::jacobian;

  JacobianWrapper()
    : work_(1)
    , scale_(dimRange)
    , rconde_(dimRange)
    , rcondv_(dimRange)
    , iwork_(2 * dimRange - 2)
    , eigenvalues_(std::vector<RangeFieldType>(dimRange))
    , tau_(V::create(dimRange))
  {
    jacobian() = std::make_unique<JacobianType>(eigenvectors_);
#if HAVE_MKL || HAVE_LAPACKE
    int ilo, ihi;
    double norm;
    if (M::storage_layout == XT::Common::StorageLayout::dense_row_major) {
      // get optimal working size in work[0] (requested by lwork = -1)
      int info = XT::Common::Lapacke::dgeevx_work(XT::Common::Lapacke::row_major(),
                                                  /*both diagonally scale and permute*/ 'B',
                                                  /*do_not_compute_left_eigenvectors:*/ 'N',
                                                  /*compute_right_eigenvectors:*/ 'V',
                                                  'N',
                                                  static_cast<int>(dimRange),
                                                  M::data(jacobian(0)),
                                                  static_cast<int>(dimRange),
                                                  &(eigenvalues_[0][0]),
                                                  &(dummy_complex_eigenvalues_[0]),
                                                  nullptr,
                                                  static_cast<int>(dimRange),
                                                  M::data(eigenvectors_[0]),
                                                  static_cast<int>(dimRange),
                                                  &ilo,
                                                  &ihi,
                                                  scale_.data(),
                                                  &norm,
                                                  rconde_.data(),
                                                  rcondv_.data(),
                                                  work_.data(),
                                                  -1,
                                                  iwork_.data());
      if (info != 0)
        DUNE_THROW(Dune::XT::LA::Exceptions::eigen_solver_failed, "The lapack backend reported '" << info << "'!");
      work_.resize(work_[0]);
    }
#endif
  }

  virtual void get_jacobian(const size_t dd,
                            const EntityType& entity,
                            const AnalyticalFluxType& analytical_flux,
                            const DomainType& x_in_inside_coords,
                            const StateRangeType& u,
                            const XT::Common::Parameter& param) override
  {
    analytical_flux.local_function(entity)->partial_u_col(dd, x_in_inside_coords, u, jacobian(dd), param);
  }

  virtual void get_jacobian(const EntityType& entity,
                            const AnalyticalFluxType& analytical_flux,
                            const DomainType& x_in_inside_coords,
                            const StateRangeType& u,
                            const XT::Common::Parameter& param) override
  {
    analytical_flux.local_function(entity)->partial_u(x_in_inside_coords, u, *jacobian(), param);
  }

  using BaseType::compute;

  virtual void compute(const size_t dd) override
  {
    if (false) {
      ;
#if HAVE_MKL || HAVE_LAPACKE
    } else if (M::storage_layout == XT::Common::StorageLayout::dense_row_major) {
      int ilo, ihi;
      double norm;
      int info = XT::Common::Lapacke::dgeevx_work(XT::Common::Lapacke::row_major(),
                                                  /*both diagonally scale and permute*/ 'B',
                                                  /*do_not_compute_left_eigenvectors:*/ 'N',
                                                  /*compute_right_eigenvectors:*/ 'V',
                                                  'N',
                                                  static_cast<int>(dimRange),
                                                  M::data(jacobian(dd)),
                                                  static_cast<int>(dimRange),
                                                  eigenvalues_[dd].data(),
                                                  &(dummy_complex_eigenvalues_[0]),
                                                  nullptr,
                                                  static_cast<int>(dimRange),
                                                  M::data(eigenvectors_[dd]),
                                                  static_cast<int>(dimRange),
                                                  &ilo,
                                                  &ihi,
                                                  scale_.data(),
                                                  &norm,
                                                  rconde_.data(),
                                                  rcondv_.data(),
                                                  work_.data(),
                                                  static_cast<int>(work_.size()),
                                                  iwork_.data());
      if (info != 0)
        DUNE_THROW(Dune::XT::LA::Exceptions::eigen_solver_failed, "The lapack backend reported '" << info << "'!");
#endif // HAVE_MKL || HAVE_LAPACKE
    } else {
      static auto eigensolver_options = BaseType::template create_eigensolver_opts<MatrixType>();
      const auto eigensolver = EigenSolverType(jacobian(dd), &eigensolver_options);
      eigenvectors_[dd] = eigensolver.real_eigenvectors();
      eigenvalues_[dd] = eigensolver.real_eigenvalues();
    }
    QR_[dd] = eigenvectors_[dd];
    XT::LA::qr(QR_[dd], tau_[dd], permutations_[dd]);
    computed_[dd] = true;
  }

  virtual void apply_eigenvectors(const size_t dd, const VectorType& x, VectorType& ret) const override
  {
    eigenvectors_[dd].mv(x, ret);
  }

  virtual void apply_inverse_eigenvectors(const size_t dd, const VectorType& x, VectorType& ret) const override
  {
    VectorType work = V::create(dimRange);
    XT::LA::solve_qr_factorized(QR_[dd], tau_[dd], permutations_[dd], ret, x, &work);
  }

protected:
  std::vector<RangeFieldType> work_, scale_, rconde_, rcondv_;
  std::vector<int> iwork_;
  using BaseType::computed_;
  JacobianType eigenvectors_;
  FieldVector<std::vector<RangeFieldType>, dimDomain> eigenvalues_;
  FieldVector<RangeFieldType, dimRange> dummy_complex_eigenvalues_;
  JacobianType QR_;
  FieldVector<VectorType, dimDomain> tau_;
  FieldVector<FieldVector<int, dimRange>, dimDomain> permutations_;
}; // class JacobianWrapper<...>

template <class AnalyticalFluxType, size_t block_size = (AnalyticalFluxType::dimDomain == 1) ? 2 : 4>
class BlockedJacobianWrapper
    : public JacobianWrapperBase<AnalyticalFluxType,
                                 FieldVector<FieldMatrix<typename AnalyticalFluxType::RangeFieldType,
                                                         block_size,
                                                         block_size>,
                                             AnalyticalFluxType::dimRange / block_size>,
                                 typename AnalyticalFluxType::StateRangeType>
{
  using BaseType =
      JacobianWrapperBase<AnalyticalFluxType,
                          FieldVector<FieldMatrix<typename AnalyticalFluxType::RangeFieldType, block_size, block_size>,
                                      AnalyticalFluxType::dimRange / block_size>,
                          typename AnalyticalFluxType::StateRangeType>;

public:
  using typename BaseType::RangeFieldType;
  using BaseType::dimDomain;
  using BaseType::dimRange;
  static constexpr size_t num_blocks = dimRange / block_size;
  static_assert(dimRange % block_size == 0, "dimRange has to be a multiple of block_size");
  using LocalRangeType = FieldVector<RangeFieldType, block_size>;
  using LocalMatrixType = FieldMatrix<RangeFieldType, block_size, block_size>;
  using MatrixType = FieldVector<LocalMatrixType, num_blocks>;
  using EigenSolverType = typename XT::LA::EigenSolver<LocalMatrixType>;
  using typename BaseType::DomainType;
  using typename BaseType::EntityType;
  using typename BaseType::JacobianType;
  using typename BaseType::StateRangeType;
  using typename BaseType::VectorType;

  BlockedJacobianWrapper()
    : work_(1)
  {
    jacobian() = std::make_unique<JacobianType>();
    nonblocked_jacobians_ = std::make_unique<FieldVector<FieldMatrix<RangeFieldType, dimRange, dimRange>, dimDomain>>();
#if HAVE_MKL || HAVE_LAPACKE
    // get optimal working size in work[0] (requested by lwork = -1)
    int info = XT::Common::Lapacke::dgeev_work(XT::Common::Lapacke::row_major(),
                                               /*do_not_compute_left_eigenvectors: */ 'N',
                                               /*compute_right_eigenvectors: */ 'V',
                                               static_cast<int>(block_size),
                                               &(jacobian(0)[0][0][0]),
                                               static_cast<int>(block_size),
                                               &(eigenvalues_[0][0][0]),
                                               &(dummy_complex_eigenvalues_[0]),
                                               nullptr,
                                               static_cast<int>(block_size),
                                               &(jacobian(0)[0][0][0]),
                                               static_cast<int>(block_size),
                                               work_.data(),
                                               -1);
    if (info != 0)
      DUNE_THROW(Dune::XT::LA::Exceptions::eigen_solver_failed, "The lapack backend reported '" << info << "'!");
    work_.resize(work_[0]);
#endif
  }

  using BaseType::jacobian;
  using BaseType::compute;

  virtual void compute(const size_t dd) override
  {
    for (size_t jj = 0; jj < num_blocks; ++jj) {
      if (block_size == 2) {
        const auto& jac = jacobian(dd)[jj];
        const auto trace = jac[0][0] + jac[1][1];
        const auto det = jac[0][0] * jac[1][1] - jac[0][1] * jac[1][0];
        const auto sqrt_val = std::sqrt(0.25 * trace * trace - det);
        const auto eigval1 = 0.5 * trace + sqrt_val;
        const auto eigval2 = 0.5 * trace - sqrt_val;
        if (std::abs(jac[1][0]) > std::abs(jac[0][1])) {
          if (XT::Common::FloatCmp::ne(jac[1][0], 0.)) {
            eigenvectors_[dd][jj][0][0] = eigval1 - jac[1][1];
            eigenvectors_[dd][jj][0][1] = eigval2 - jac[1][1];
            eigenvectors_[dd][jj][1][0] = eigenvectors_[dd][jj][1][1] = jac[1][0];
          } else {
            eigenvectors_[dd][jj][0][0] = eigenvectors_[dd][jj][1][1] = 1.;
            eigenvectors_[dd][jj][0][1] = eigenvectors_[dd][jj][1][0] = 0.;
          }
        } else {
          if (XT::Common::FloatCmp::ne(jac[0][1], 0.)) {
            eigenvectors_[dd][jj][1][0] = eigval1 - jac[0][0];
            eigenvectors_[dd][jj][1][1] = eigval2 - jac[0][0];
            eigenvectors_[dd][jj][0][0] = eigenvectors_[dd][jj][0][1] = jac[0][1];
          } else {
            eigenvectors_[dd][jj][0][0] = eigenvectors_[dd][jj][1][1] = 1.;
            eigenvectors_[dd][jj][0][1] = eigenvectors_[dd][jj][1][0] = 0.;
          }
        }
      } else {
#if HAVE_MKL || HAVE_LAPACKE
        int info = XT::Common::Lapacke::dgeev_work(XT::Common::Lapacke::row_major(),
                                                   /*do_not_compute_left_eigenvectors: */ 'N',
                                                   /*compute_right_eigenvectors: */ 'V',
                                                   static_cast<int>(block_size),
                                                   &(jacobian(dd)[jj][0][0]),
                                                   static_cast<int>(block_size),
                                                   &(eigenvalues_[jj][0][0]),
                                                   &(dummy_complex_eigenvalues_[0]),
                                                   nullptr,
                                                   static_cast<int>(block_size),
                                                   &(eigenvectors_[dd][jj][0][0]),
                                                   static_cast<int>(block_size),
                                                   work_.data(),
                                                   static_cast<int>(work_.size()));
        if (info != 0)
          DUNE_THROW(Dune::XT::LA::Exceptions::eigen_solver_failed, "The lapack backend reported '" << info << "'!");
#else // HAVE_MKL || HAVE_LAPACKE
        static XT::Common::Configuration eigensolver_options =
            BaseType::template create_eigensolver_opts<LocalMatrixType>();
        const auto eigensolver = EigenSolverType(jacobian(dd)[jj], &eigensolver_options);
        eigenvectors_[dd][jj] = eigensolver.real_eigenvectors();
#endif // HAVE_MKL || HAVE_LAPACKE
      } // else (block_size == 2)
      QR_[dd][jj] = eigenvectors_[dd][jj];
      XT::LA::qr(QR_[dd][jj], tau_[dd][jj], permutations_[dd][jj]);
    } // jj
    computed_[dd] = true;
  }

  virtual void apply_eigenvectors(const size_t dd, const StateRangeType& x, StateRangeType& ret) const override
  {
    std::fill(ret.begin(), ret.end(), 0.);
    for (size_t jj = 0; jj < num_blocks; ++jj) {
      const auto offset = block_size * jj;
      for (size_t ll = 0; ll < block_size; ++ll)
        for (size_t mm = 0; mm < block_size; ++mm)
          ret[offset + ll] += eigenvectors_[dd][jj][ll][mm] * x[offset + mm];
    } // jj
  }

  virtual void get_jacobian(const size_t dd,
                            const EntityType& entity,
                            const AnalyticalFluxType& analytical_flux,
                            const DomainType& x_in_inside_coords,
                            const StateRangeType& u,
                            const XT::Common::Parameter& param) override
  {
    const auto local_func = analytical_flux.local_function(entity);
    local_func->partial_u_col(dd, x_in_inside_coords, u, (*nonblocked_jacobians_)[dd], param);
    for (size_t jj = 0; jj < num_blocks; ++jj) {
      const auto offset = jj * block_size;
      for (size_t ll = 0; ll < block_size; ++ll)
        for (size_t mm = 0; mm < block_size; ++mm)
          jacobian(dd)[jj][ll][mm] = (*nonblocked_jacobians_)[dd][offset + ll][offset + mm];
    } // jj
  }

  virtual void get_jacobian(const EntityType& entity,
                            const AnalyticalFluxType& analytical_flux,
                            const DomainType& x_in_inside_coords,
                            const StateRangeType& u,
                            const XT::Common::Parameter& param) override
  {
    const auto local_func = analytical_flux.local_function(entity);
    local_func->partial_u(x_in_inside_coords, u, *nonblocked_jacobians_, param);
    for (size_t dd = 0; dd < dimDomain; ++dd) {
      for (size_t jj = 0; jj < num_blocks; ++jj) {
        const auto offset = jj * block_size;
        for (size_t ll = 0; ll < block_size; ++ll)
          for (size_t mm = 0; mm < block_size; ++mm)
            jacobian(dd)[jj][ll][mm] = (*nonblocked_jacobians_)[dd][offset + ll][offset + mm];
      } // jj
    } // dd
  }

  virtual void apply_inverse_eigenvectors(const size_t dd, const VectorType& x, VectorType& ret) const override
  {
    LocalRangeType work;
    LocalRangeType tmp_ret, tmp_x;
    for (size_t jj = 0; jj < num_blocks; ++jj) {
      for (size_t ll = 0; ll < block_size; ++ll)
        tmp_x[ll] = x[jj * block_size + ll];
      XT::LA::solve_qr_factorized(QR_[dd][jj], tau_[dd][jj], permutations_[dd][jj], tmp_ret, tmp_x, &work);
      for (size_t ll = 0; ll < block_size; ++ll)
        ret[jj * block_size + ll] = tmp_ret[ll];
    }
  }

protected:
  std::vector<RangeFieldType> work_;
  std::unique_ptr<FieldVector<FieldMatrix<RangeFieldType, dimRange, dimRange>, dimDomain>> nonblocked_jacobians_;
  using BaseType::computed_;
  FieldVector<FieldVector<FieldVector<RangeFieldType, block_size>, num_blocks>, dimDomain> eigenvalues_;
  FieldVector<RangeFieldType, dimRange> dummy_complex_eigenvalues_;
  FieldVector<MatrixType, dimDomain> eigenvectors_;
  FieldVector<MatrixType, dimDomain> QR_;
  FieldVector<FieldVector<LocalRangeType, num_blocks>, dimDomain> tau_;
  FieldVector<FieldVector<FieldVector<int, block_size>, num_blocks>, dimDomain> permutations_;
}; // BlockedJacobianWrapper<...>

} // namespace internal

template <class MultiArrayType>
class MultiIndexIterator
{
public:
  static constexpr size_t dim = MultiArrayType::dimensionality;
  static constexpr size_t array_dim = dim == 0 ? 1 : dim;
  using IndicesType = std::array<size_t, array_dim>;

  MultiIndexIterator(const MultiArrayType& multi_array, const IndicesType& indices)
    : multi_array_(multi_array)
    , indices_(indices)
  {
  }

  IndicesType& operator*()
  {
    return indices_;
  }

  const IndicesType& operator*() const
  {
    return indices_;
  }

  MultiIndexIterator& operator++()
  {
    size_t ii = 0;
    for (; ii < array_dim; ++ii) {
      indices_[ii]++;
      if (indices_[ii] < (dim == 0 ? 1 : multi_array_.shape()[ii]))
        break;
      indices_[ii] = 0;
    }
    if (ii == array_dim) {
      if (dim == 0)
        indices_[0] = 1;
      else
        std::copy_n(multi_array_.shape(), array_dim, indices_.begin());
    }
    return *this;
  } // ... operator++()

  MultiIndexIterator operator++(int)
  {
    MultiIndexIterator ret = *this;
    this->operator++();
    return ret;
  } // ... operator++(int)

  bool operator==(const MultiIndexIterator& other) const
  {
    return indices_ == other.indices_;
  }

  bool operator!=(const MultiIndexIterator& other) const
  {
    return indices_ != other.indices_;
  }

private:
  const MultiArrayType& multi_array_;
  IndicesType indices_;
}; // class MultiIndexIterator<...>

template <class MultiArrayType>
class MultiIndexProvider
{
public:
  using IteratorType = MultiIndexIterator<MultiArrayType>;
  using IndicesType = typename IteratorType::IndicesType;

  MultiIndexProvider(const MultiArrayType& multi_array)
    : multi_array_(multi_array)
  {
  }

  IteratorType begin()
  {
    static const IndicesType zero_indices = []() {
      IndicesType ret;
      ret.fill(0);
      return ret;
    }();
    return IteratorType(multi_array_, zero_indices);
  }

  IteratorType end()
  {
    IndicesType indices;
    if (MultiArrayType::dimensionality == 0)
      indices[0] = 1; // for dimension 0, the end iterator has index 1
    else
      std::copy_n(multi_array_.shape(), MultiArrayType::dimensionality, indices.begin());
    return IteratorType(multi_array_, indices);
  }

private:
  const MultiArrayType& multi_array_;
}; // class MultiIndexProvider<...>

// Helper functor to build indices.
template <typename RangeArrayType, size_t num_ranges, size_t dimension>
struct IndicesBuilder
{
  static boost::detail::multi_array::index_gen<num_ranges, dimension> build(const RangeArrayType& ranges)
  {
    return boost::detail::multi_array::index_gen<num_ranges, dimension>(
        IndicesBuilder<RangeArrayType, num_ranges - 1, dimension>::build(ranges), ranges[num_ranges - 1]);
  }
};

// Helper functor specialization to terminate recursion.
template <typename RangeArrayType, size_t dimension>
struct IndicesBuilder<RangeArrayType, 0, dimension>
{
  static boost::detail::multi_array::index_gen<0, dimension> build(const RangeArrayType& /*ranges*/)
  {
    return boost::detail::multi_array::index_gen<0, dimension>();
  }
};

template <class RangeType, size_t dimDomain>
class Slice : public boost::multi_array<RangeType, dimDomain - 1>
{
};

template <class RangeType>
class Slice<RangeType, 1>
{
public:
  template <class MultiIndexType>
  RangeType& operator()(const MultiIndexType&)
  {
    return value_;
  }

  template <class IndicesType>
  void resize(const IndicesType&)
  {
  }

private:
  RangeType value_;
};

template <class AnalyticalFluxType,
          class BoundaryValueType,
          class GridLayerType,
          SlopeLimiters slope_limiter,
          class JacobianWrapperType>
class LocalLinearReconstructionOperator : public XT::Grid::Functor::Codim0<GridLayerType>
{
  // stencil is (i-r, i+r) in all dimensions, where r = polOrder + 1
  static constexpr size_t dimDomain = BoundaryValueType::dimDomain;
  static constexpr size_t dimRange = BoundaryValueType::dimRange;
  static constexpr size_t axis_size = 3;
  typedef typename GridLayerType::template Codim<0>::Entity EntityType;
  typedef typename GridLayerType::IndexSet IndexSetType;
  typedef typename BoundaryValueType::DomainType DomainType;
  typedef typename BoundaryValueType::DomainFieldType DomainFieldType;
  typedef typename BoundaryValueType::RangeType RangeType;
  typedef typename BoundaryValueType::RangeFieldType RangeFieldType;
  typedef Dune::QuadratureRule<DomainFieldType, 1> Quadrature1dType;
  typedef typename GridLayerType::Intersection IntersectionType;
  typedef FieldVector<IntersectionType, 2 * dimDomain> IntersectionVectorType;
  typedef typename IntersectionType::Geometry::LocalCoordinate IntersectionLocalCoordType;
  typedef typename AnalyticalFluxType::LocalfunctionType AnalyticalFluxLocalfunctionType;
  typedef typename AnalyticalFluxLocalfunctionType::StateRangeType StateRangeType;
  typedef typename XT::Grid::BoundaryInfo<IntersectionType> BoundaryInfoType;
  using ReconstructedFunctionType =
      ReconstructedLocalizableFunction<GridLayerType, DomainFieldType, dimDomain, RangeFieldType, dimRange, 1>;
  using StencilType = boost::multi_array<boost::optional<RangeType>, dimDomain>;
  using MultiArrayType = boost::multi_array<RangeType, dimDomain>;
  using SliceType = Slice<RangeType, dimDomain>;
  using CoordsType = std::array<size_t, dimDomain>;

public:
  explicit LocalLinearReconstructionOperator(const std::vector<RangeType>& source_values,
                                             const AnalyticalFluxType& analytical_flux,
                                             const BoundaryValueType& boundary_values,
                                             const GridLayerType& grid_layer,
                                             const XT::Common::Parameter& param,
                                             const Quadrature1dType& quadrature,
                                             ReconstructedFunctionType& reconstructed_function,
                                             XT::Common::PerThreadValue<JacobianWrapperType>& jacobian_wrapper)
    : source_values_(source_values)
    , analytical_flux_(analytical_flux)
    , boundary_values_(boundary_values)
    , grid_layer_(grid_layer)
    , param_(param)
    , quadrature_(quadrature)
    , reconstructed_function_(reconstructed_function)
    , jacobian_wrapper_(jacobian_wrapper)
  {
    param_.set("boundary", {0.});
  }

  virtual void apply_local(const EntityType& entity) override final
  {
    static const CoordsType stencil_sizes = []() {
      CoordsType ret;
      const auto ax_size = axis_size; // avoid linker error
      ret.fill(ax_size);
      return ret;
    }();
    thread_local StencilType stencil(stencil_sizes);
    bool valid = fill_stencil(stencil, entity);
    // In a MPI parallel run, if entity is on boundary of overlap, we do not have to reconstruct
    if (!valid)
      return;
    // get intersections
    FieldVector<typename GridLayerType::Intersection, 2 * dimDomain> intersections;
    for (const auto& intersection : Dune::intersections(grid_layer_, entity))
      intersections[intersection.indexInInside()] = intersection;
    const auto entity_index = grid_layer_.indexSet().index(entity);
    auto& reconstructed_values_map = reconstructed_function_.values()[entity_index];

    // get jacobian
    auto& jac = *jacobian_wrapper_;
    if (!jac.computed() || !analytical_flux_.is_affine()) {
      const auto& u_entity = source_values_[entity_index];
      const DomainType x_in_inside_coords = entity.geometry().local(entity.geometry().center());
      jac.get_jacobian(entity, analytical_flux_, x_in_inside_coords, u_entity, param_);
      jac.compute();
      if (analytical_flux_.is_affine())
        jac.jacobian() = nullptr;
    }

    for (size_t dd = 0; dd < dimDomain; ++dd) {
      if (quadrature_.size() == 1) {
        // no need to reconstruct in all directions, as we are only regarding the center of the face, which will always
        // have the same value assigned, independent of the slope in the other directions
        std::array<size_t, dimDomain> indices;
        indices.fill(1);
        FieldVector<RangeType, axis_size> stencil_1d;
        FieldVector<RangeType, 2> reconstructed_values;
        for (size_t ii = 0; ii < axis_size; ++ii) { // transform to characteristic variables
          indices[dd] = ii;
          jac.apply_inverse_eigenvectors(dd, *stencil(indices), stencil_1d[ii]);
        }
        // perform the actual reconstruction
        linear_reconstruction_1d(stencil_1d, reconstructed_values);
        // convert back to non-characteristic variables
        auto tmp_value = reconstructed_values[0];
        jac.apply_eigenvectors(dd, tmp_value, reconstructed_values[0]);
        tmp_value = reconstructed_values[1];
        jac.apply_eigenvectors(dd, tmp_value, reconstructed_values[1]);

        // store reconstructed values
        reconstructed_values_map.emplace(intersections[2 * dd].geometryInInside().center(), reconstructed_values[0]);
        reconstructed_values_map.emplace(intersections[2 * dd + 1].geometryInInside().center(),
                                         reconstructed_values[1]);
      } else {
        thread_local MultiArrayType reconstructed_values(stencil_sizes);
        thread_local auto tmp_multiarray = reconstructed_values;
        tmp_multiarray.resize(stencil_sizes);

        // Transform values on stencil to characteristic variables of the current coordinate direction dd
        for (size_t ii = 0; ii < stencil.num_elements(); ++ii)
          jac.apply_inverse_eigenvectors(dd, *stencil.data()[ii], tmp_multiarray.data()[ii]);

        size_t curr_dir = dd;
        size_t last_dir = curr_dir;
        RangeType tmp_value;
        CoordsType current_sizes;
        for (size_t dir = 0; dir < dimDomain; ++dir) {
          curr_dir = (dd + dir) % dimDomain;
          // Transform to characteristic variables of the current reconstruction direction.
          if (dir > 0) {
            std::copy_n(reconstructed_values.shape(), dimDomain, current_sizes.begin());
            tmp_multiarray.resize(current_sizes);
            tmp_multiarray = reconstructed_values;
            std::for_each(
                tmp_multiarray.data(), tmp_multiarray.data() + tmp_multiarray.num_elements(), [&](RangeType& value) {
                  jac.apply_eigenvectors(last_dir, value, tmp_value);
                  jac.apply_inverse_eigenvectors(curr_dir, tmp_value, value);
                });
          } // if (dir > 0)
          // perform the actual reconstruction
          const auto& curr_quadrature = dir > 0 ? quadrature_ : get_left_right_quadrature();
          linear_reconstruction(curr_dir, curr_quadrature, tmp_multiarray, reconstructed_values);
          last_dir = curr_dir;
        } // dir
        // convert back to non-characteristic variables
        std::for_each(reconstructed_values.data(),
                      reconstructed_values.data() + reconstructed_values.num_elements(),
                      [&](RangeType& value) {
                        tmp_value = value;
                        jac.apply_eigenvectors(last_dir, tmp_value, value);
                      });

        // Convert coordinates on face to local entity coordinates and store reconstructed values
        MultiIndexProvider<MultiArrayType> multi_indices(reconstructed_values);
        for (const auto& multi_index : multi_indices) {
          IntersectionLocalCoordType quadrature_point;
          for (size_t ii = 0; ii < dimDomain; ++ii)
            if (ii != dd)
              quadrature_point[ii < dd ? ii : ii - 1] = quadrature_[multi_index[ii]].position();
          reconstructed_values_map.emplace(
              intersections[2 * dd + multi_index[dd]].geometryInInside().global(quadrature_point),
              reconstructed_values(multi_index));
        } // multi_indices
      }
    } // dd
  } // void apply_local(...)

  static void linear_reconstruction_1d(const FieldVector<RangeType, axis_size>& stencil,
                                       FieldVector<RangeType, 2>& reconstructed_values)
  {
    const auto& u_left = stencil[0];
    const auto& u_entity = stencil[1];
    const auto& u_right = stencil[2];
    const auto slope_left = u_entity - u_left;
    const auto slope_center = 0.5 * (u_right - u_left);
    const auto slope_right = u_right - u_entity;
    auto slope = internal::ChooseLimiter<slope_limiter>::limit(slope_left, slope_right, slope_center);
    slope *= 0.5;
    reconstructed_values[0] = u_entity - slope;
    reconstructed_values[1] = u_entity + slope;
  } // static void linear_reconstruction(...)

  static void linear_reconstruction(const size_t dir,
                                    const Quadrature1dType& quadrature,
                                    const MultiArrayType& stencil,
                                    MultiArrayType& reconstructed_values)
  {
    // resize the reconstructed_values array
    CoordsType new_shape;
    std::copy_n(stencil.shape(), dimDomain, new_shape.begin());
    new_shape[dir] = quadrature.size();
    reconstructed_values.resize(new_shape);

    // get array_views corresponding to the left, center and right values
    using IndexRangeType = typename MultiArrayType::index_range;
    using RangeArrayType = XT::Common::FieldVector<IndexRangeType, dimDomain>;
    using IndicesBuilderType = IndicesBuilder<RangeArrayType, dimDomain, dimDomain - 1>;

    assert(stencil.shape()[dir] == 3);
    RangeArrayType ranges;
    for (size_t ii = 0; ii < dimDomain; ++ii)
      ranges[ii] = IndexRangeType(0, stencil.shape()[ii]);
    ranges[dir] = IndexRangeType(0);
    const auto u_left = stencil[IndicesBuilderType::build(ranges)];
    ranges[dir] = IndexRangeType(1);
    const auto u_entity = stencil[IndicesBuilderType::build(ranges)];
    ranges[dir] = IndexRangeType(2);
    const auto u_right = stencil[IndicesBuilderType::build(ranges)];

    // calculate slopes
    thread_local SliceType slope;
    std::array<size_t, dimDomain - 1> slope_sizes;
    std::copy_n(u_entity.shape(), dimDomain - 1, slope_sizes.begin());
    slope.resize(slope_sizes);
    MultiIndexProvider<decltype(u_left)> multi_indices(u_left);
    for (const auto& multi_index : multi_indices) {
      const auto slope_left = u_entity(multi_index) - u_left(multi_index);
      const auto slope_center = 0.5 * (u_right(multi_index) - u_left(multi_index));
      const auto slope_right = u_right(multi_index) - u_entity(multi_index);
      slope(multi_index) = internal::ChooseLimiter<slope_limiter>::limit(slope_left, slope_right, slope_center);
    }

    // calculate reconstructed values
    for (size_t ii = 0; ii < dimDomain; ++ii)
      ranges[ii] = IndexRangeType(0, new_shape[ii]);
    for (size_t ii = 0; ii < quadrature.size(); ++ii) {
      ranges[dir] = IndexRangeType(ii);
      auto reconstructed_ii = reconstructed_values[IndicesBuilderType::build(ranges)];
      for (const auto& multi_index : multi_indices) {
        reconstructed_ii(multi_index) = slope(multi_index);
        reconstructed_ii(multi_index) *= (quadrature[ii].position() - 0.5);
        reconstructed_ii(multi_index) += u_entity(multi_index);
      }
    } // ii (quadrature.size())
  } // static void linear_reconstruction(...)

private:
  CoordsType center(const StencilType& stencil) const
  {
    CoordsType ret;
    for (size_t ii = 0; ii < dimDomain; ++ii) {
      assert(stencil.shape()[ii] % 2 && "Center not well-defined if one of the axis_sizes is even!");
      ret[ii] = stencil.shape()[ii] / 2;
    }
    return ret;
  }

  bool fill_stencil(StencilType& stencil, const EntityType& entity)
  {
    const int dir = -2;
    auto coords = center(stencil);
    std::fill_n(stencil.data(), stencil.num_elements(), boost::none);
    return fill_impl(stencil, entity, dir, coords);
  } // void fill(...)

private:
  bool fill_impl(StencilType& stencil, const EntityType& entity, const int dir, const CoordsType& coords)
  {
    bool ret = true;
    const auto entity_index = grid_layer_.indexSet().index(entity);
    stencil(coords) = source_values_[entity_index];
    std::vector<int> boundary_dirs;
    for (const auto& intersection : Dune::intersections(grid_layer_, entity)) {
      const auto new_dir = intersection.indexInInside();
      if (direction_allowed(dir, new_dir) && !end_of_stencil(stencil, new_dir, coords)) {
        auto new_coords = coords;
        if (intersection.boundary() && !intersection.neighbor()) { // boundary intersections
          boundary_dirs.push_back(new_dir);
          auto boundary_value = boundary_values_.local_function(entity)->evaluate(
              intersection, entity.geometry().local(intersection.geometry().center()), source_values_[entity_index]);
          while (!end_of_stencil(stencil, new_dir, new_coords)) {
            next_coords_in_dir(new_dir, new_coords);
            stencil(new_coords) = boundary_value;
          }
        } else if (intersection.neighbor()) { // inner and periodic intersections
          const auto& outside = intersection.outside();
          next_coords_in_dir(new_dir, new_coords);
          ret = ret && fill_impl(stencil, outside, new_dir, new_coords);
        } else if (!intersection.neighbor() && !intersection.boundary()) { // processor boundary
          return false;
        }
      } // if (!end_of_stencil(...))
    } // intersections

    assert(boundary_dirs.size() <= dimDomain);
    if (boundary_dirs.size() > 1) {
      auto new_coords = coords;
      next_coords_in_dir(boundary_dirs[0], new_coords);
      const auto& boundary_value = stencil(new_coords);

      std::for_each(stencil.data(),
                    stencil.data() + stencil.num_elements(),
                    [&boundary_value](boost::optional<RangeType>& value) {
                      if (!value)
                        value = boundary_value;
                    });
    } // if (boundary_dirs.size() > 1)
    return ret;
  }

  //  get next coords in direction dir (increase or decrease coords in that direction)
  static void next_coords_in_dir(const int dir, CoordsType& coords)
  {
    dir % 2 ? coords[dir / 2]++ : coords[dir / 2]--;
  }

  // Direction is allowed if end of stencil is not reached and direction is not visited by another iterator.
  // Iterators never change direction, they may only spawn new iterators in the directions that have a higher
  // index (i.e. iterators that walk in x direction will spawn iterators going in y and z direction,
  // iterators going in y direction will only spawn iterators in z-direction and z iterators only walk
  // without emitting new iterators).
  static bool direction_allowed(const int dir, const int new_dir)
  {
    return new_dir == dir || new_dir / 2 > dir / 2;
  }

  bool end_of_stencil(const StencilType& stencil, const int dir, const CoordsType& coords)
  {
    return coords[dir / 2] == stencil.shape()[dir / 2] - 1 || coords[dir / 2] == 0;
  }

  // quadrature rule containing left and right interface points
  static const Quadrature1dType& get_left_right_quadrature()
  {
    static const Quadrature1dType ret = left_right_quadrature();
    return ret;
  }

  static Quadrature1dType left_right_quadrature()
  {
    Quadrature1dType ret;
    ret.push_back(Dune::QuadraturePoint<DomainFieldType, 1>(0., 0.5));
    ret.push_back(Dune::QuadraturePoint<DomainFieldType, 1>(1., 0.5));
    return ret;
  }

  const std::vector<RangeType>& source_values_;
  const AnalyticalFluxType& analytical_flux_;
  const BoundaryValueType& boundary_values_;
  const GridLayerType& grid_layer_;
  XT::Common::Parameter param_;
  const Quadrature1dType& quadrature_;
  ReconstructedFunctionType& reconstructed_function_;
  XT::Common::PerThreadValue<JacobianWrapperType>& jacobian_wrapper_;
}; // class LocalLinearReconstructionOperator

template <class AnalyticalFluxImp,
          class BoundaryValueImp,
          SlopeLimiters slope_limiter,
          class JacobianWrapperImp,
          class Traits>
class LinearReconstructionOperator;

namespace internal {

template <class AnalyticalFluxImp, class BoundaryValueImp, SlopeLimiters slope_lim, class JacobianWrapperImp>
struct LinearReconstructionOperatorTraits
{
  using AnalyticalFluxType = AnalyticalFluxImp;
  using BoundaryValueType = BoundaryValueImp;
  static const SlopeLimiters slope_limiter = slope_lim;
  using JacobianWrapperType = JacobianWrapperImp;
  using DomainFieldType = typename BoundaryValueType::DomainFieldType;
  using RangeFieldType = typename BoundaryValueType::DomainFieldType;
  using FieldType = DomainFieldType;
  using JacobianType = NoJacobian;
  static constexpr size_t dimDomain = BoundaryValueType::dimDomain;
  static constexpr size_t dimRange = BoundaryValueType::dimRange;
  using ProductQuadratureType = QuadratureRule<DomainFieldType, dimDomain - 1>;
  using Quadrature1dType = Dune::QuadratureRule<DomainFieldType, 1>;
  using derived_type = LinearReconstructionOperator<AnalyticalFluxType,
                                                    BoundaryValueType,
                                                    slope_limiter,
                                                    JacobianWrapperType,
                                                    LinearReconstructionOperatorTraits>;
};

} // namespace internal

template <class AnalyticalFluxImp,
          class BoundaryValueImp,
          SlopeLimiters slope_limiter = SlopeLimiters::minmod,
          class JacobianWrapperImp = internal::JacobianWrapper<AnalyticalFluxImp,
                                                               FieldMatrix<typename BoundaryValueImp::RangeFieldType,
                                                                           BoundaryValueImp::dimRange,
                                                                           BoundaryValueImp::dimRange>,
                                                               FieldVector<typename BoundaryValueImp::RangeFieldType,
                                                                           BoundaryValueImp::dimRange>>,
          class Traits = internal::LinearReconstructionOperatorTraits<AnalyticalFluxImp,
                                                                      BoundaryValueImp,
                                                                      slope_limiter,
                                                                      JacobianWrapperImp>>
class LinearReconstructionOperator : public OperatorInterface<Traits>
{
public:
  using AnalyticalFluxType = typename Traits::AnalyticalFluxType;
  using BoundaryValueType = typename Traits::BoundaryValueType;
  using JacobianWrapperType = typename Traits::JacobianWrapperType;
  using Quadrature1dType = typename Traits::Quadrature1dType;
  using ProductQuadratureType = typename Traits::ProductQuadratureType;
  using DomainFieldType = typename Traits::DomainFieldType;
  using RangeFieldType = typename Traits::RangeFieldType;
  static constexpr size_t dimDomain = Traits::dimDomain;
  static constexpr size_t dimRange = Traits::dimRange;

  LinearReconstructionOperator(const AnalyticalFluxType& analytical_flux,
                               const BoundaryValueType& boundary_values,
                               const Quadrature1dType& quadrature_1d = default_1d_quadrature<DomainFieldType>(1))
    : analytical_flux_(analytical_flux)
    , boundary_values_(boundary_values)
    , quadrature_1d_(quadrature_1d)
    , product_quadrature_(product_quadrature_on_intersection<DomainFieldType, dimDomain>(quadrature_1d))
  {
  }

  const ProductQuadratureType& quadrature() const
  {
    return product_quadrature_;
  }

  template <class SourceType, class RangeType>
  void apply(const SourceType& source, RangeType& range, const XT::Common::Parameter& param) const
  {
    static_assert(is_reconstructed_localizable_function<RangeType>::value,
                  "RangeType has to be derived from ReconstructedLocalizableFunction!");
    // evaluate cell averages
    const auto& grid_layer = source.space().grid_layer();
    const auto& index_set = grid_layer.indexSet();
    std::vector<typename SourceType::RangeType> source_values(index_set.size(0));
    for (const auto& entity : Dune::elements(grid_layer)) {
      const auto entity_index = index_set.index(entity);
      const auto local_source = source.local_function(entity);
      source_values[entity_index] = local_source->evaluate(entity.geometry().local(entity.geometry().center()));
    }

    // do reconstruction
    auto local_reconstruction_operator =
        LocalLinearReconstructionOperator<AnalyticalFluxType,
                                          BoundaryValueType,
                                          typename SourceType::SpaceType::GridLayerType,
                                          slope_limiter,
                                          JacobianWrapperType>(source_values,
                                                               analytical_flux_,
                                                               boundary_values_,
                                                               grid_layer,
                                                               param,
                                                               quadrature_1d_,
                                                               range,
                                                               jacobian_wrapper_);
    auto walker = XT::Grid::Walker<typename SourceType::SpaceType::GridLayerType>(grid_layer);
    walker.append(local_reconstruction_operator);
    walker.walk(true);
  } // void apply(...)

private:
  const AnalyticalFluxType& analytical_flux_;
  const BoundaryValueType& boundary_values_;
  const Quadrature1dType quadrature_1d_;
  const ProductQuadratureType product_quadrature_;
  mutable XT::Common::PerThreadValue<JacobianWrapperType> jacobian_wrapper_;
}; // class LinearReconstructionOperator<...>

} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_OPERATORS_FV_RECONSTRUCTION_LINEAR_HH


#include <fstream>
#include <boost/program_options.hpp>
#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <iostream>
#include <algorithm>
#include <tbb/mutex.h>
#include <mutex>
#include <tbb/blocked_range.h>
#include <tbb/parallel_sort.h>
#include <tbb/tbb.h>
#include <unordered_map>
#include "tree.hpp"

namespace po = boost::program_options;

std::vector<Node*> get_ordered_leaves(Tree T) {
    auto dfs = T.depth_first_expansion();
    std::vector<Node*> ordered_leaves;
    for (auto n: dfs) {
        if (n->is_leaf()) {
            ordered_leaves.push_back(n);
        }
    }
    return ordered_leaves;
}

float get_average_rank (std::vector<Node*> nodes, std::unordered_map<std::string, size_t> rank_dict) {
    float num = 0;
    float den = 1e-9;

    for (auto n: nodes) {
        if (rank_dict.find(n->identifier) != rank_dict.end()) {
            num += static_cast<float>(rank_dict[n->identifier]);
            den += 1.0;
        }
    }

    return (num/den);
}

struct Node_rank {
    Node* node;
    float rank;
    Node_rank(Node* n, float r) : node(n), rank(r)
    {
    }
};

bool compare_by_rank (const Node_rank& nr1, const Node_rank nr2) {
    return nr1.rank < nr2.rank;
}

uint32_t rotate_T2 (std::vector<Node*> T1_ordered_leaves, Tree& T2) {

    std::unordered_map<std::string, size_t> rank_dict;

    for (size_t k=0; k<T1_ordered_leaves.size(); k++) {
        rank_dict[T1_ordered_leaves[k]->identifier] = k;
    }

    uint32_t num_rot = 0;

    auto dfs = T2.depth_first_expansion();
    size_t num_nodes = dfs.size();

    tbb::mutex data_lock;

    tbb::parallel_for( tbb::blocked_range<size_t>(0, num_nodes, 20),
            [&](tbb::blocked_range<size_t> r) {

            for (size_t k=r.begin(); k<r.end(); ++k) {
               auto curr_node = dfs[k];
               std::vector<Node_rank> children_ranks;
               for (auto c: curr_node->children) {
                   data_lock.lock();
                   auto leaves = T2.get_leaves(c->identifier);
                   data_lock.unlock();
                   float rank = get_average_rank(leaves, rank_dict);
                   children_ranks.push_back(Node_rank(c, rank));
               }
               if (!is_sorted(children_ranks.begin(), children_ranks.end(), compare_by_rank)) {
                    std::sort(children_ranks.begin(), children_ranks.end(), compare_by_rank);

                    data_lock.lock();
                    num_rot++;
                    curr_node->children.clear();
                    for (auto cr: children_ranks) {
                        curr_node->children.push_back(cr.node);
                    }
                    data_lock.unlock();
               }

            }
            });

    return num_rot;
}

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

    std::string T1_filename;
    std::string T2_filename;
    std::string T1_out_filename;
    std::string T2_out_filename;
    uint32_t max_iter;

    po::options_description desc{"Options"};
    desc.add_options()
        ("T1", po::value<std::string>(&T1_filename)->required(), "Input tree1 file")
        ("T2", po::value<std::string>(&T2_filename)->required(), "Input tree2 file")
        ("T1_out", po::value<std::string>(&T1_out_filename)->required(), "Output rotated tree1 file")
        ("T2_out", po::value<std::string>(&T2_out_filename)->required(), "Output rotated tree2 file")
        ("max_iter", po::value<uint32_t>(&max_iter)->default_value(1e2), "Maximum number of iterations")
        ("help", "Print help messages");

    po::options_description all_options;
    all_options.add(desc);

    po::positional_options_description p;
    p.add("T1", 1);
    p.add("T2", 1);
    p.add("T1_out", 1);
    p.add("T2_out", 1);
    p.add("max_iter", 1);

    po::variables_map vm;
    try{
        po::store(po::command_line_parser(argc, argv).options(all_options).positional(p).run(), vm);
        po::notify(vm);
    }
    catch(std::exception &e){
        std::cout << desc << std::endl;
        if(vm.count("help"))
            return 0;
        else
            return 1;
    }

    uint32_t num_cores = tbb::task_scheduler_init::default_num_threads();
    tbb::task_scheduler_init init(num_cores);

    Tree T1, T2, T1_out, T2_out;

    T1 = TreeLib::create_tree_from_newick(T1_filename);
    T2 = TreeLib::create_tree_from_newick(T2_filename);

    uint32_t n1=1, n2=1;
    uint32_t num_iter=0;

    while ((n1+n2 > 0) && (num_iter < max_iter)) {
        auto T1_ordered_leaves = get_ordered_leaves(T1);
        n1 = rotate_T2(T1_ordered_leaves, T2);

        auto T2_ordered_leaves = get_ordered_leaves(T2);
        n2 = rotate_T2(T2_ordered_leaves, T1);

        num_iter++;
        fprintf(stderr, "===Iteration %u===\nPerformed %u rotations.\n", num_iter, n1+n2);
    }

    if ((num_iter == max_iter) && (n1+n2>0)) {
        fprintf(stderr, "\nWARNING: Reached maximum iterations before converging.\n");
    }

    std::ofstream ofile1, ofile2;
    ofile1.open(T1_out_filename);
    ofile2.open(T2_out_filename);

    ofile1 << TreeLib::get_newick_string(T1, false, true);
    ofile2 << TreeLib::get_newick_string(T2, false, true);

    ofile1.close();
    ofile2.close();

    return 0;
}


#ifndef BOOST_SAFE_FLOAT_POLICY_CHECK_ADDITION_INVALID_RESULT_HPP
#define BOOST_SAFE_FLOAT_POLICY_CHECK_ADDITION_INVALID_RESULT_HPP

#include <boost/safe_float/policy/check_base_policy.hpp>

#ifdef FENV_AVAILABLE
#pragma STDC FENV_ACCESS ON
#include <fenv.h>
#else
#include <cmath>
#endif

namespace boost {
namespace safe_float{
namespace policy{

template<class FP>
class check_addition_invalid_result : public check_policy<FP> {
public:
    bool pre_addition_check(const FP& lhs, const FP& rhs){
#ifndef FENV_AVAILABLE
        return true;
#else
        return ! std::feclearexcept(FE_INVALID);
#endif
    }
    bool post_addition_check(const FP& rhs){
#ifndef FENV_AVAILABLE
        return !std::isnan(rhs);
#else
        return ! std::fetestexcept(FE_INVALID);
#endif
    }
    std::string addition_failure_message(){
        return std::string("Invalid result from arithmetic operation obtained");
    }
};

}
}
}

#endif // BOOST_SAFE_FLOAT_POLICY_CHECK_ADDITION_INVALID_RESULT_HPP


#include <sm/eigen/gtest.hpp>
#include <Eigen/Geometry>
#include <aslam/backend/Scalar.hpp>
#include <aslam/backend/VectorExpression.hpp>
#include <aslam/backend/DesignVariableVector.hpp>
#include <aslam/backend/test/ExpressionTests.hpp>

using namespace aslam::backend;
using namespace std;

TEST(VectorExpressionNodeTestSuites, testVectorBasicOperations) {
  try {
    const int VEC_ROWS = 5;
    const int VEC_ROWS2 = 3;
    const int VEC_ROWS3 = 2;

    typedef VectorExpression<VEC_ROWS> VEC;
    typedef VectorExpression<Eigen::Dynamic> VEC2;
    typedef VectorExpression<Eigen::Dynamic> VEC3;

    DesignVariableVector<VEC_ROWS> vec(VEC::value_t::Random());
    VEC2::value_t vec2 = VEC2::value_t::Random(VEC_ROWS2);
    DesignVariableVector<Eigen::Dynamic> vec3(VEC3::value_t::Random(VEC_ROWS3));
    VEC vecExp(&vec);
    VEC2 vecExp2(vec2);
    auto vecExp3 = vec3.toExpression();

    EXPECT_EQ(VEC_ROWS, vec.getSize());
    EXPECT_EQ(VEC_ROWS, vecExp.getSize());
    EXPECT_EQ(VEC_ROWS2, vecExp2.getSize());
    EXPECT_EQ(VEC_ROWS3, vec3.getSize());
    EXPECT_EQ(VEC_ROWS3, vecExp3.getSize());

    sm::eigen::assertNear(vecExp.evaluate(), vec.value(), 1e-14, SM_SOURCE_FILE_POS, "Testing evaluation fits initialization.");
    sm::eigen::assertNear(vecExp2.evaluate(), vec2, 1e-14, SM_SOURCE_FILE_POS, "Testing evaluation fits initialization.");
    sm::eigen::assertNear(vecExp3.evaluate(), vec3.value(), 1e-14, SM_SOURCE_FILE_POS, "Testing evaluation fits initialization.");

    testExpression(vecExp, 1);
    testExpression(vecExp2, 0);
    testExpression(vecExp3, 1);
  }
  catch(std::exception const & e)
  {
    FAIL() << e.what();
  }
}


#include <cassert>
#include <cmath>
#include <algorithm>
#include <boost/functional/hash.hpp>

#include "whisker.hh"

using namespace std;

Whisker::Whisker( const unsigned int s_window_increment, const double s_window_multiple, const double s_intersend, const MemoryRange & s_domain )
  : Action(s_domain),
    _window_increment( s_window_increment ),
    _window_multiple( s_window_multiple ),
    _intersend( s_intersend )
{
}

Whisker::Whisker( const Whisker & other )
  : Action( other ),
    _window_increment( other._window_increment ),
    _window_multiple( other._window_multiple ),
    _intersend( other._intersend )
{
}

Whisker::Whisker( const RemyBuffers::Whisker & dna )
  : Action( dna.domain() ),
    _window_increment( dna.window_increment() ),
    _window_multiple( dna.window_multiple() ),
    _intersend( dna.intersend() )
{
}

RemyBuffers::Whisker Whisker::DNA( void ) const
{
  RemyBuffers::Whisker ret;

  ret.set_window_increment( _window_increment );
  ret.set_window_multiple( _window_multiple );
  ret.set_intersend( _intersend );
  ret.mutable_domain()->CopyFrom( _domain.DNA() );

  return ret;
}

vector< Whisker > Whisker::next_generation( bool optimize_window_increment, bool optimize_window_multiple, bool optimize_intersend ) const
{
  vector< Whisker> ret;

  auto window_increment_alternatives = get_optimizer().window_increment.alternatives( _window_increment, optimize_window_increment );
  auto window_multiple_alternatives = get_optimizer().window_multiple.alternatives( _window_multiple, optimize_window_multiple );
  auto intersend_alternatives = get_optimizer().intersend.alternatives( _intersend, optimize_intersend );

  printf("Alternatives: window increment %u to %u, window multiple %f to %f, intersend %f to %f\n",
         *(min_element(window_increment_alternatives.begin(), window_increment_alternatives.end())),
         *(max_element(window_increment_alternatives.begin(), window_increment_alternatives.end())),
         *(min_element(window_multiple_alternatives.begin(), window_multiple_alternatives.end())),
         *(max_element(window_multiple_alternatives.begin(), window_multiple_alternatives.end())),
         *(min_element(intersend_alternatives.begin(), intersend_alternatives.end())),
         *(max_element(intersend_alternatives.begin(), intersend_alternatives.end()))
  );

  for ( const auto & alt_window : window_increment_alternatives ) {
    for ( const auto & alt_multiple : window_multiple_alternatives ) {
      for ( const auto & alt_intersend : intersend_alternatives ) {
        Whisker new_whisker { *this };
        new_whisker._generation++;

        new_whisker._window_increment = alt_window;
        new_whisker._window_multiple = alt_multiple;
        new_whisker._intersend = alt_intersend;

        new_whisker.round();

        ret.push_back( new_whisker );
      }
    }
  }

  return ret;
}

string Whisker::str( const unsigned int total ) const
{
  char tmp[ 256 ];
  snprintf( tmp, 256, "{%s} gen=%u usage=%.4f => (win=%d + %f * win, intersend=%f)",
	    _domain.str().c_str(), _generation, double( _domain.count() ) / double( total ), _window_increment, _window_multiple, _intersend );
  return tmp;
}

void Whisker::round( void )
{
  _window_multiple = (1.0/10000.0) * int( 10000 * _window_multiple );
  _intersend = (1.0/10000.0) * int( 10000 * _intersend );
}

size_t hash_value( const Whisker & whisker )
{
  size_t seed = 0;
  boost::hash_combine( seed, whisker._window_increment );
  boost::hash_combine( seed, whisker._window_multiple );
  boost::hash_combine( seed, whisker._intersend );
  boost::hash_combine( seed, whisker._domain );

  return seed;
}


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#ifndef CBR_CONTROL__PID_HPP_
#define CBR_CONTROL__PID_HPP_

#include <boost/hana/adapt_struct.hpp>

#include <cbr_utils/cyber_timer.hpp>

#include <sophus/so2.hpp>

#include <memory>
#include <type_traits>
#include <utility>

#include "cbr_control/derivator.hpp"

namespace cbr
{

struct PIDClampParams
{
  bool active{true};
  double max{1.};
  double min{-1.};

  void check_correctness() const
  {
    if (min > max) {
      throw std::invalid_argument("max must be less or equal to min");
    }
  }
};

struct PIDParams
{
  double P{1.};
  double I{1.};
  double D{1.};

  double der_tau{0.};

  PIDClampParams clamp;

  void check_correctness() const
  {
    clamp.check_correctness();
  }
};

template<typename _clock_t = std::chrono::high_resolution_clock>
class PID
{
public:
  using clock_t = _clock_t;
  using timer_t = CyberTimerNoAvg<std::ratio<1>, double, clock_t>;
  using derivator_t = Derivator<double, clock_t>;

  // Constructors
  PID() = default;
  PID(const PID &) = default;
  PID(PID &) = default;
  PID(PID &&) = default;
  PID & operator=(const PID &) = default;
  PID & operator=(PID &) = default;
  PID & operator=(PID &&) = default;
  ~PID() = default;

  template<typename T>
  explicit PID(T && clock, const PIDParams & prm = {}, const double errInt = 0.)
  : timer_(std::forward<T>(clock)),
    prm_(prm),
    errInt_(errInt)
  {
    prm_.check_correctness();
    der_.set_params({prm_.der_tau});
  }

  explicit PID(const PIDParams & prm, const double errInt = 0.)
  : prm_(prm),
    errInt_(errInt)
  {
    prm_.check_correctness();
    der_.set_params({prm_.der_tau});
  }

  template<typename T>
  PID(T && clock, const double errInt)
  : timer_(std::forward<T>(clock)),
    errInt_(errInt)
  {}

  explicit PID(const double errInt)
  : errInt_(errInt)
  {}

  template<typename T>
  void set_clock(T && clock)
  {
    timer_.set_clock(std::forward<T>(clock));
  }

  void set_params(const PIDParams & prm)
  {
    prm.check_correctness();
    prm_ = prm;
    der_.set_params({prm_.der_tau});
  }

  const PIDParams & get_params() const
  {
    return prm_;
  }

  const double & update(const double desired, const double actual)
  {
    return update_impl(desired - actual, actual);
  }

  const double & update(const Sophus::SO2d & desired, const Sophus::SO2d & actual)
  {
    return update_impl((actual.inverse() * desired).log(), actual.log());
  }

  const double & operator()(const double desired, const double actual)
  {
    return update(desired, actual);
  }

  const double & operator()(const Sophus::SO2d & desired, const Sophus::SO2d & actual)
  {
    return update(desired, actual);
  }

  const double & getValue() const
  {
    return output_;
  }

  void reset(const double errInt = 0.)
  {
    der_.reset();
    errInt_ = prm_.I * errInt;
    if (prm_.clamp.active) {
      errInt_ = std::clamp(errInt_, prm_.clamp.min, prm_.clamp.max);
    }
    output_ = errInt_;
    timer_.tic();
  }

protected:
  const double & update_impl(const double err, const double actual)
  {
    const auto tNow = timer_.now();
    const double errDot = der_.update(actual, tNow);
    output_ = prm_.P * err + prm_.D * errDot + errInt_;

    if (init_) {
      const double dt = timer_.toctic(tNow);
      const double dt_err = dt * err * prm_.I;
      const double errIntTmp = errInt_ + dt_err;
      const double outputTmp = output_ + dt_err;
      if (!prm_.clamp.active || (outputTmp < prm_.clamp.max && outputTmp > prm_.clamp.min)) {
        errInt_ = errIntTmp;
        output_ = outputTmp;
      }
    } else {
      init_ = true;
      timer_.tic(tNow);
    }

    if (prm_.clamp.active) {
      output_ = std::clamp(output_, prm_.clamp.min, prm_.clamp.max);
    }
    return output_;
  }

  timer_t timer_ = timer_t(clock_t{});
  PIDParams prm_;
  double errInt_{0.};
  double output_{0.};
  bool init_{false};
  derivator_t der_;
};

}    // namespace cbr

// cppcheck-suppress unknownMacro
BOOST_HANA_ADPAT_STRUCT(
  cbr::PIDClampParams,
  active,
  max,
  min
);

// cppcheck-suppress unknownMacro
BOOST_HANA_ADAPT_STRUCT(
  cbr::PIDParams,
  P,
  I,
  D,
  der_tau,
  clamp
);

#endif  // CBR_CONTROL__PID_HPP_


/*******************************************************************
 * 文件名:mocap_formation_control.cpp
 *
 * 作者: BOSHEN97
 *
 * 更新时间: 2020.11.20
 *
 * 介绍:该cpp文件主要为动捕集群中四机绕圈相关函数的实现以及程序的运行
 * ****************************************************************/
#include "ros/ros.h"
#include "Formation.h"
#include <unistd.h>
#include <Eigen/Eigen>
#include <mavros_msgs/PositionTarget.h>

void formation::init()
{

	//集群四台飞机位置控制数据发布者
  	uav1_local_pub = n.advertise<mavros_msgs::PositionTarget>("/uav1/mavros/setpoint_raw/local", 10);
  	uav2_local_pub = n.advertise<mavros_msgs::PositionTarget>("/uav2/mavros/setpoint_raw/local", 10);
  	uav3_local_pub = n.advertise<mavros_msgs::PositionTarget>("/uav3/mavros/setpoint_raw/local", 10);
  	uav4_local_pub = n.advertise<mavros_msgs::PositionTarget>("/uav4/mavros/setpoint_raw/local", 10);

	//读取相关参数
	ros::param::param<double>("~square_length", square_length, 4);
	ros::param::param<double>("~13_height", point13_height, 1);
	ros::param::param<double>("~24_height", point24_height, 0.5);
	ros::param::param<double>("~hold_time", hold_time, 10);
	ros::param::param<double>("~stage1_time", stage1_time, 5);
	ros::param::param<int>("~LAND_intervals", land_intervals, 3);
	ros::param::param<bool>("~sim", sim, false);
	ros::param::param<double>("uav1_x", uav1_gazebo_offset_pose[0], 0);
  	ros::param::param<double>("uav1_y", uav1_gazebo_offset_pose[1], 0);
  	ros::param::param<double>("uav2_x", uav2_gazebo_offset_pose[0], 0);
  	ros::param::param<double>("uav2_y", uav2_gazebo_offset_pose[1], 0);
  	ros::param::param<double>("uav3_x", uav3_gazebo_offset_pose[0], 0);
  	ros::param::param<double>("uav3_y", uav3_gazebo_offset_pose[1], 0);
  	ros::param::param<double>("uav4_x", uav4_gazebo_offset_pose[0], 0);
  	ros::param::param<double>("uav4_y", uav4_gazebo_offset_pose[1], 0);
}

void formation::is_sim()
{
    if(!sim)
    {
        uav1_gazebo_offset_pose[0] = 0;
        uav1_gazebo_offset_pose[1] = 0;

        uav2_gazebo_offset_pose[0] = 0;
        uav2_gazebo_offset_pose[1] = 0;

        uav3_gazebo_offset_pose[0] = 0;
        uav3_gazebo_offset_pose[1] = 0;

        uav4_gazebo_offset_pose[0] = 0;
        uav4_gazebo_offset_pose[1] = 0;
    }
}

void formation::square()
{
	init();
	is_sim();
	while(ros::ok())
	{
		std::cout << "Please input 1 start square formation" << std::endl;
		int start_flag;
		std::cin >> start_flag;
		if(start_flag == 1)
		{
			//阶段1
			int stage = 1;
			if(stage == 1)
			{
				int count = 0;
				int num = (stage1_time + hold_time) * 10;
				uav1_desired_pose.type_mask = 0b100111111000;
				uav1_desired_pose.coordinate_frame = 1;
				uav1_desired_pose.position.x = square_length/2 - uav1_gazebo_offset_pose[0];
				uav1_desired_pose.position.y = square_length/2 - uav1_gazebo_offset_pose[1];
				uav1_desired_pose.position.z = point13_height;

				uav2_desired_pose.type_mask = 0b100111111000;
				uav2_desired_pose.coordinate_frame = 1;
				uav2_desired_pose.position.x = square_length/2 - uav2_gazebo_offset_pose[0];
				uav2_desired_pose.position.y = -square_length/2 - uav2_gazebo_offset_pose[1];
				uav2_desired_pose.position.z = point24_height;

				uav3_desired_pose.type_mask = 0b100111111000;
				uav3_desired_pose.coordinate_frame = 1;
				uav3_desired_pose.position.x = -square_length/2 - uav3_gazebo_offset_pose[0];
				uav3_desired_pose.position.y = -square_length/2 - uav3_gazebo_offset_pose[1];
				uav3_desired_pose.position.z = point13_height;

				uav4_desired_pose.type_mask = 0b100111111000;
				uav4_desired_pose.coordinate_frame = 1;
				uav4_desired_pose.position.x = -square_length/2 - uav4_gazebo_offset_pose[0];
				uav4_desired_pose.position.y = square_length/2 - uav4_gazebo_offset_pose[1];
				uav4_desired_pose.position.z = point24_height;

				while(ros::ok())
				{
					uav1_desired_pose.header.stamp = ros::Time::now();
					uav1_local_pub.publish(uav1_desired_pose);
					uav2_desired_pose.header.stamp = ros::Time::now();
					uav2_local_pub.publish(uav2_desired_pose);
					uav3_desired_pose.header.stamp = ros::Time::now();
					uav3_local_pub.publish(uav3_desired_pose);
					uav4_desired_pose.header.stamp = ros::Time::now();
					uav4_local_pub.publish(uav4_desired_pose);
					if(count == 0)
					{
						set_formation_px4_offboard();
					}
					count++;
					if(count >= num)
					{
						stage = 2;
						break;
					}
					ROS_INFO("go to point 1");
					usleep(100000);
				}
			}

			if(stage == 2)
			{
				int count = 0;
				int num = hold_time * 10;
				uav1_desired_pose.type_mask = 0b100111111000;
				uav1_desired_pose.coordinate_frame = 1;
				uav1_desired_pose.position.x = square_length/2 - uav1_gazebo_offset_pose[0];
				uav1_desired_pose.position.y = -square_length/2 - uav1_gazebo_offset_pose[1];
				uav1_desired_pose.position.z = point24_height;

				uav2_desired_pose.type_mask = 0b100111111000;
				uav2_desired_pose.coordinate_frame = 1;
				uav2_desired_pose.position.x = -square_length/2 - uav2_gazebo_offset_pose[0];
				uav2_desired_pose.position.y = -square_length/2 - uav2_gazebo_offset_pose[1];
				uav2_desired_pose.position.z = point13_height;

				uav3_desired_pose.type_mask = 0b100111111000;
				uav3_desired_pose.coordinate_frame = 1;
				uav3_desired_pose.position.x = -square_length/2 - uav3_gazebo_offset_pose[0];
				uav3_desired_pose.position.y = square_length/2 - uav3_gazebo_offset_pose[1];
				uav3_desired_pose.position.z = point24_height;

				uav4_desired_pose.type_mask = 0b100111111000;
				uav4_desired_pose.coordinate_frame = 1;
				uav4_desired_pose.position.x = square_length/2 - uav4_gazebo_offset_pose[0];
				uav4_desired_pose.position.y = square_length/2 - uav4_gazebo_offset_pose[1];
				uav4_desired_pose.position.z = point13_height;

				while(ros::ok())
				{
					uav1_desired_pose.header.stamp = ros::Time::now();
					uav1_local_pub.publish(uav1_desired_pose);
					uav2_desired_pose.header.stamp = ros::Time::now();
					uav2_local_pub.publish(uav2_desired_pose);
					uav3_desired_pose.header.stamp = ros::Time::now();
					uav3_local_pub.publish(uav3_desired_pose);
					uav4_desired_pose.header.stamp = ros::Time::now();
					uav4_local_pub.publish(uav4_desired_pose);
					count++;
					if(count >= num)
					{
						stage = 3;
						break;
					}
					ROS_INFO("go to point 2");
					usleep(100000);
				}
			}

			if(stage == 3)
			{
				int count = 0;
				int num = hold_time * 10;
				uav1_desired_pose.type_mask = 0b100111111000;
				uav1_desired_pose.coordinate_frame = 1;
				uav1_desired_pose.position.x = -square_length/2 - uav1_gazebo_offset_pose[0];
				uav1_desired_pose.position.y = -square_length/2 - uav1_gazebo_offset_pose[1];
				uav1_desired_pose.position.z = point13_height;

				uav2_desired_pose.type_mask = 0b100111111000;
				uav2_desired_pose.coordinate_frame = 1;
				uav2_desired_pose.position.x = -square_length/2 - uav2_gazebo_offset_pose[0];
				uav2_desired_pose.position.y = square_length/2 - uav2_gazebo_offset_pose[1];
				uav2_desired_pose.position.z = point24_height;

				uav3_desired_pose.type_mask = 0b100111111000;
				uav3_desired_pose.coordinate_frame = 1;
				uav3_desired_pose.position.x = square_length/2 - uav3_gazebo_offset_pose[0];
				uav3_desired_pose.position.y = square_length/2 - uav3_gazebo_offset_pose[1];
				uav3_desired_pose.position.z = point13_height;

				uav4_desired_pose.type_mask = 0b100111111000;
				uav4_desired_pose.coordinate_frame = 1;
				uav4_desired_pose.position.x = square_length/2 - uav4_gazebo_offset_pose[0];
				uav4_desired_pose.position.y = -square_length/2 - uav4_gazebo_offset_pose[1];
				uav4_desired_pose.position.z = point24_height;

				while(ros::ok())
				{
					uav1_desired_pose.header.stamp = ros::Time::now();
					uav1_local_pub.publish(uav1_desired_pose);
					uav2_desired_pose.header.stamp = ros::Time::now();
					uav2_local_pub.publish(uav2_desired_pose);
					uav3_desired_pose.header.stamp = ros::Time::now();
					uav3_local_pub.publish(uav3_desired_pose);
					uav4_desired_pose.header.stamp = ros::Time::now();
					uav4_local_pub.publish(uav4_desired_pose);
					count++;
					if(count >= num)
					{
						stage = 4;
						break;
					}
					ROS_INFO("go to point 3");
					usleep(100000);
				}
			}

			if(stage == 4)
			{
				int count = 0;
				int num = hold_time * 10;
				uav1_desired_pose.type_mask = 0b100111111000;
				uav1_desired_pose.coordinate_frame = 1;
				uav1_desired_pose.position.x = -square_length/2 - uav1_gazebo_offset_pose[0];
				uav1_desired_pose.position.y = square_length/2 - uav1_gazebo_offset_pose[1];
				uav1_desired_pose.position.z = point24_height;

				uav2_desired_pose.type_mask = 0b100111111000;
				uav2_desired_pose.coordinate_frame = 1;
				uav2_desired_pose.position.x = square_length/2 - uav2_gazebo_offset_pose[0];
				uav2_desired_pose.position.y = square_length/2 - uav2_gazebo_offset_pose[1];
				uav2_desired_pose.position.z = point13_height;

				uav3_desired_pose.type_mask = 0b100111111000;
				uav3_desired_pose.coordinate_frame = 1;
				uav3_desired_pose.position.x = square_length/2 - uav3_gazebo_offset_pose[0];
				uav3_desired_pose.position.y = -square_length/2 - uav3_gazebo_offset_pose[1];
				uav3_desired_pose.position.z = point24_height;

				uav4_desired_pose.type_mask = 0b100111111000;
				uav4_desired_pose.coordinate_frame = 1;
				uav4_desired_pose.position.x = -square_length/2 - uav4_gazebo_offset_pose[0];
				uav4_desired_pose.position.y = -square_length/2 - uav4_gazebo_offset_pose[1];
				uav4_desired_pose.position.z = point13_height;

				while(ros::ok())
				{
					uav1_desired_pose.header.stamp = ros::Time::now();
					uav1_local_pub.publish(uav1_desired_pose);
					uav2_desired_pose.header.stamp = ros::Time::now();
					uav2_local_pub.publish(uav2_desired_pose);
					uav3_desired_pose.header.stamp = ros::Time::now();
					uav3_local_pub.publish(uav3_desired_pose);
					uav4_desired_pose.header.stamp = ros::Time::now();
					uav4_local_pub.publish(uav4_desired_pose);
					count++;
					if(count >= num)
					{
						stage = 5;
						break;
					}
					ROS_INFO("go to point 4");
					usleep(100000);
				}
			}

			if(stage == 5)
			{
				int count = 0;
				int num = hold_time * 10;
				uav1_desired_pose.type_mask = 0b100111111000;
				uav1_desired_pose.coordinate_frame = 1;
				uav1_desired_pose.position.x = square_length/2 - uav1_gazebo_offset_pose[0];
				uav1_desired_pose.position.y = square_length/2 - uav1_gazebo_offset_pose[1];
				uav1_desired_pose.position.z = point13_height;

				uav2_desired_pose.type_mask = 0b100111111000;
				uav2_desired_pose.coordinate_frame = 1;
				uav2_desired_pose.position.x = square_length/2 - uav2_gazebo_offset_pose[0];
				uav2_desired_pose.position.y = -square_length/2 - uav2_gazebo_offset_pose[1];
				uav2_desired_pose.position.z = point24_height;

				uav3_desired_pose.type_mask = 0b100111111000;
				uav3_desired_pose.coordinate_frame = 1;
				uav3_desired_pose.position.x = -square_length/2 - uav3_gazebo_offset_pose[0];
				uav3_desired_pose.position.y = -square_length/2 - uav3_gazebo_offset_pose[1];
				uav3_desired_pose.position.z = point13_height;

				uav4_desired_pose.type_mask = 0b100111111000;
				uav4_desired_pose.coordinate_frame = 1;
				uav4_desired_pose.position.x = -square_length/2 - uav4_gazebo_offset_pose[0];
				uav4_desired_pose.position.y = square_length/2 - uav4_gazebo_offset_pose[1];
				uav4_desired_pose.position.z = point24_height;

				while(ros::ok())
				{
					uav1_desired_pose.header.stamp = ros::Time::now();
					uav1_local_pub.publish(uav1_desired_pose);
					uav2_desired_pose.header.stamp = ros::Time::now();
					uav2_local_pub.publish(uav2_desired_pose);
					uav3_desired_pose.header.stamp = ros::Time::now();
					uav3_local_pub.publish(uav3_desired_pose);
					uav4_desired_pose.header.stamp = ros::Time::now();
					uav4_local_pub.publish(uav4_desired_pose);
					count++;
					if(count >= num)
					{
						//四机降落
						set_formation_px4_land();
						break;
					}
					ROS_INFO("go to point 1");
					usleep(100000);
				}
			}
			break;
		}
		else
		{
			ROS_WARN("input error, please input again");
		}
	}
}

void formation::is_wait(int time)
{
    //判断是否需要等待,time变量不为0,则等待
    if(time != 0)
    {
        sleep(time);
    }
}

void formation::set_formation_px4_offboard()
{
    //创建1~4号机的command_to_mavros类
    command_to_mavros ctm1("uav1");

    command_to_mavros ctm2("uav2");

    command_to_mavros ctm3("uav3");

    command_to_mavros ctm4("uav4");

    //设置1~4号机模式变量为offboard,解上锁变量为解锁
    ctm1.arm_cmd.request.value = true;
    ctm2.arm_cmd.request.value = true;
    ctm3.arm_cmd.request.value = true;
    ctm4.arm_cmd.request.value = true;

    ctm1.mode_cmd.request.custom_mode = "OFFBOARD";
    ctm2.mode_cmd.request.custom_mode = "OFFBOARD";
    ctm3.mode_cmd.request.custom_mode = "OFFBOARD";
    ctm4.mode_cmd.request.custom_mode = "OFFBOARD";

    //集群按照1234的顺序对五台无人机分别解锁并切入offboard模式
    //当有一台无人机解锁或者切入offboard模式失败,该函数返回false
    //五台无人机成功解锁并切入offboard模式,该函数返回true
    if(ctm1.arming_client.call(ctm1.arm_cmd) && ctm1.set_mode_client.call(ctm1.mode_cmd))
    {
        ROS_INFO("uav1 armed and set offboard mode success");
        is_wait(offboard_intervals);
    }
    else
    {
        ROS_ERROR("uav1 armed and set offboard mode failed");
    }

    if(ctm2.arming_client.call(ctm2.arm_cmd) && ctm2.set_mode_client.call(ctm2.mode_cmd))
    {
        ROS_INFO("uav2 armed and set offboard mode success");
        is_wait(offboard_intervals);
    }
    else
    {
        ROS_ERROR("uav2 armed and set offboard mode failed");
    }

    if(ctm3.arming_client.call(ctm3.arm_cmd) && ctm3.set_mode_client.call(ctm3.mode_cmd))
    {
        ROS_INFO("uav3 armed and set offboard mode success");
        is_wait(offboard_intervals);
    }
    else
    {
        ROS_ERROR("uav3 armed and set offboard mode failed");
    }

    if(ctm4.arming_client.call(ctm4.arm_cmd) && ctm4.set_mode_client.call(ctm4.mode_cmd))
    {
        ROS_INFO("uav4 armed and set offboard mode success");
        is_wait(offboard_intervals);
    }
    else
    {
        ROS_ERROR("uav4 armed and set offboard mode failed");
    }

}

void formation::set_formation_px4_land()
{
    //创建1~4号机的command_to_mavros类
    command_to_mavros ctm1("uav1");

    command_to_mavros ctm2("uav2");

    command_to_mavros ctm3("uav3");

    command_to_mavros ctm4("uav4");

    //设置1~4号机模式变量为land
    ctm1.mode_cmd.request.custom_mode = "AUTO.LAND";
    ctm2.mode_cmd.request.custom_mode = "AUTO.LAND";
    ctm3.mode_cmd.request.custom_mode = "AUTO.LAND";
    ctm4.mode_cmd.request.custom_mode = "AUTO.LAND";

    //切换为land模式,并对结果进行打印
	if(ctm1.set_mode_client.call(ctm1.mode_cmd))
    {
        ROS_INFO("uav1 set land mode success");
        is_wait(land_intervals);
    }
    else
    {
        ROS_ERROR("uav1 set land mode failed");
    }

    if(ctm2.set_mode_client.call(ctm2.mode_cmd))
    {
        ROS_INFO("uav2 set land mode success");
        is_wait(land_intervals);
    }
    else
    {
        ROS_ERROR("uav2 set land mode failed");
    }

    if(ctm3.set_mode_client.call(ctm3.mode_cmd))
    {
        ROS_INFO("uav3 set land mode success");
        is_wait(land_intervals);
    }
    else
    {
        ROS_ERROR("uav3 set land mode failed");
    }

    if(ctm4.set_mode_client.call(ctm4.mode_cmd))
    {
        ROS_INFO("uav4 set land mode success");
        is_wait(land_intervals);
    }
    else
    {
        ROS_ERROR("uav4 set land mode success");
    }
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "formation_square");
	formation Formation;
	Formation.square();
	return 0;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2022 Aleksei Moskvin <alalmoskvin@nil.foundation>
//
// MIT License
//
// 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.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE pickles_struct_test

#include <string>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

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

#include <nil/crypto3/multiprecision/cpp_int.hpp>

#include <nil/crypto3/algebra/curves/vesta.hpp>
#include <nil/crypto3/algebra/curves/pallas.hpp>

#include <boost/foreach.hpp>

#include <nil/crypto3/zk/snark/systems/plonk/pickles/proof.hpp>
#include <nil/crypto3/zk/snark/systems/plonk/pickles/verifier_index.hpp>

using namespace nil::crypto3;

BOOST_AUTO_TEST_SUITE(kimchi_proof_struct_test_suite)

template<typename Iterator>
nil::crypto3::multiprecision::cpp_int get_cppui256(Iterator it) {
    BOOST_ASSERT(it->second.template get_value<std::string>() != "");
    return nil::crypto3::multiprecision::cpp_int(it->second.template get_value<std::string>());
}

zk::snark::pickles_proof<nil::crypto3::algebra::curves::vesta> fill_proof(boost::property_tree::ptree root) {
    zk::snark::pickles_proof<nil::crypto3::algebra::curves::vesta> proof;
    size_t i = 0;
    std::string base_path = "protocolStateProof.json.proof.";

    auto best_chain = *root.get_child("data.bestChain").begin();
    i = 0;
    for (auto &row : best_chain.second.get_child(base_path + "messages.w_comm")) {
        auto it = row.second.get_child("").begin()->second.get_child("").begin();
        proof.commitments.w_comm[i].unshifted.emplace_back(get_cppui256(it++), get_cppui256(it));
        ++i;
    }
    auto it = best_chain.second.get_child(base_path + "messages.z_comm").begin()->second.get_child("").begin();
    proof.commitments.z_comm.unshifted.emplace_back(get_cppui256(it++), get_cppui256(it));

    it = best_chain.second.get_child(base_path + "messages.t_comm").begin()->second.get_child("").begin();
    proof.commitments.t_comm.unshifted.emplace_back(get_cppui256(it++), get_cppui256(it));
    //    proof.commitments.lookup;    // TODO: where it is?

    i = 0;
    for (auto &row : best_chain.second.get_child(base_path + "openings.proof.lr")) {
        auto it0 = row.second.begin()->second.get_child("").begin();
        auto it1 = row.second.begin();
        it1++;
        it1 = it1->second.begin();
        proof.proof.lr.push_back({{get_cppui256(it0++), get_cppui256(it0)}, {get_cppui256(it1++), get_cppui256(it1)}});
        ++i;
    }
    it = best_chain.second.get_child(base_path + "openings.proof.delta").begin();
    proof.proof.delta = {get_cppui256(it++), get_cppui256(it)};
    it = best_chain.second.get_child(base_path + "openings.proof.sg").begin();
    proof.proof.sg = {get_cppui256(it++), get_cppui256(it)};

    proof.proof.z1 = multiprecision::cpp_int(best_chain.second.get<std::string>(base_path + "openings.proof.z_1"));
    proof.proof.z2 = multiprecision::cpp_int(best_chain.second.get<std::string>(base_path + "openings.proof.z_2"));

    std::size_t ev_i = 0;
    for (auto &evals_it : best_chain.second.get_child(base_path + "openings.evals")) {

        i = 0;
        for (auto &row : evals_it.second.get_child("w")) {
            proof.evals[ev_i].w[i] = get_cppui256(row.second.begin());
        }

        proof.evals[ev_i].z = get_cppui256(evals_it.second.get_child("z").begin());

        i = 0;
        for (auto &row : evals_it.second.get_child("s")) {
            proof.evals[ev_i].s[i] = get_cppui256(row.second.begin());
        }
        proof.evals[ev_i].generic_selector = get_cppui256(evals_it.second.get_child("generic_selector").begin());
        proof.evals[ev_i].poseidon_selector = get_cppui256(evals_it.second.get_child("poseidon_selector").begin());

        ev_i++;
    }

    proof.ft_eval1 = multiprecision::cpp_int(best_chain.second.get<std::string>(base_path + "openings.ft_eval1"));
    //            // public
    //            std::vector<typename CurveType::scalar_field_type::value_type> public_p; // TODO: where it is?
    //
    //            // Previous challenges
    //            std::vector<
    //                std::tuple<std::vector<typename CurveType::scalar_field_type::value_type>, commitment_scheme>>
    //                prev_challenges; // TODO: where it is?
    return proof;
}

zk::snark::verifier_index<nil::crypto3::algebra::curves::vesta>
    fill_verify_index(boost::property_tree::ptree root, boost::property_tree::ptree const_root) {
    zk::snark::verifier_index<nil::crypto3::algebra::curves::vesta> ver_index;
    size_t i = 0;
    ver_index.domain = {const_root.get<std::size_t>("verify_index.domain.log_size_of_group"),
                        multiprecision::cpp_int(const_root.get<std::string>("verify_index.domain.group_gen"))};

    ver_index.max_poly_size = root.get<std::size_t>("data.blockchainVerificationKey.index.max_poly_size");
    ver_index.max_quot_size = root.get<std::size_t>("data.blockchainVerificationKey.index.max_quot_size");
    //    ver_index.srs = root.get<std::string>("data.blockchainVerificationKey.index.srs");    // TODO: null
    i = 0;
    for (auto &row : root.get_child("data.blockchainVerificationKey.commitments.sigma_comm")) {
        auto it = row.second.begin();
        ver_index.sigma_comm[i].unshifted.emplace_back(get_cppui256(it++), get_cppui256(it));
        ++i;
    }

    i = 0;
    for (auto &row : root.get_child("data.blockchainVerificationKey.commitments.coefficients_comm")) {
        auto it = row.second.begin();
        ver_index.coefficients_comm[i].unshifted.emplace_back(get_cppui256(it++), get_cppui256(it));
        ++i;
    }
    auto it = root.get_child("data.blockchainVerificationKey.commitments.generic_comm").begin();
    ver_index.generic_comm.unshifted.emplace_back(get_cppui256(it++), get_cppui256(it));

    it = root.get_child("data.blockchainVerificationKey.commitments.psm_comm").begin();
    ver_index.psm_comm.unshifted.emplace_back(get_cppui256(it++), get_cppui256(it));
    it = root.get_child("data.blockchainVerificationKey.commitments.complete_add_comm").begin();
    ver_index.complete_add_comm.unshifted.emplace_back(get_cppui256(it++), get_cppui256(it));
    it = root.get_child("data.blockchainVerificationKey.commitments.mul_comm").begin();
    ver_index.mul_comm.unshifted.emplace_back(get_cppui256(it++), get_cppui256(it));
    it = root.get_child("data.blockchainVerificationKey.commitments.emul_comm").begin();
    ver_index.emul_comm.unshifted.emplace_back(get_cppui256(it++), get_cppui256(it));
    it = root.get_child("data.blockchainVerificationKey.commitments.endomul_scalar_comm").begin();
    ver_index.endomul_scalar_comm.unshifted.emplace_back(get_cppui256(it++), get_cppui256(it));

    // TODO: null in example
    //    i = 0;
    //    for (auto &row : root.get_child("data.blockchainVerificationKey.commitments.chacha_comm")) {
    //        auto it = row.second.begin();
    //        ver_index.chacha_comm[i].unshifted.emplace_back(get_cppui256(it++), get_cppui256(it));
    //        ++i;
    //    }
    i = 0;
    for (auto &row : root.get_child("data.blockchainVerificationKey.index.shifts")) {
        ver_index.shifts[i] = multiprecision::cpp_int(row.second.get_value<std::string>());
        ++i;
    }

    // Polynomial in coefficients form
    // Const
    ver_index.zkpm = {0x2C46205451F6C3BBEA4BABACBEE609ECF1039A903C42BFF639EDC5BA33356332_cppui256,
                      0x1764D9CB4C64EBA9A150920807637D458919CB6948821F4D15EB1994EADF9CE3_cppui256,
                      0x0140117C8BBC4CE4644A58F7007148577782213065BB9699BF5C391FBE1B3E6D_cppui256,
                      0x0000000000000000000000000000000000000000000000000000000000000001_cppui256};
    ver_index.w = multiprecision::cpp_int(const_root.get<std::string>("verify_index.w"));
    ver_index.endo = multiprecision::cpp_int(const_root.get<std::string>("verify_index.endo"));

    //ver_index.lookup_index = root.get_child("data.blockchainVerificationKey.index.lookup_index"); // TODO: null
    //ver_index.linearization;       // TODO: where it is?
    ver_index.powers_of_alpha.next_power = 24;

    for (auto &row : const_root.get_child("verify_index.fr_sponge_params.round_constants")) {
        auto it = row.second.begin();
        ver_index.fr_sponge_params.round_constants.push_back(
            {get_cppui256(it++), get_cppui256(it++), get_cppui256(it)});
    }
    for (auto &row : const_root.get_child("verify_index.fr_sponge_params.mds")) {
        auto it = row.second.begin();
        ver_index.fr_sponge_params.mds.push_back({get_cppui256(it++), get_cppui256(it++), get_cppui256(it)});
    }

    for (auto &row : const_root.get_child("verify_index.fq_sponge_params.round_constants")) {
        auto it = row.second.begin();
        ver_index.fq_sponge_params.round_constants.push_back(
            {get_cppui256(it++), get_cppui256(it++), get_cppui256(it)});
    }
    for (auto &row : const_root.get_child("verify_index.fq_sponge_params.mds")) {
        auto it = row.second.begin();
        ver_index.fq_sponge_params.mds.push_back({get_cppui256(it++), get_cppui256(it++), get_cppui256(it)});
    }
    return ver_index;
}

BOOST_AUTO_TEST_CASE(pickles_proof_struct_test_suite) {
    boost::property_tree::ptree root;
    boost::property_tree::ptree const_root;
    // Load the json file in this ptree
    std::string test_data = TEST_DATA;
    boost::property_tree::read_json(test_data + ".json", root);
    boost::property_tree::read_json(test_data + "_const.json", const_root);

    zk::snark::pickles_proof<nil::crypto3::algebra::curves::vesta> proof = fill_proof(root);
    zk::snark::verifier_index<nil::crypto3::algebra::curves::vesta> ver_index = fill_verify_index(root, const_root);
}
BOOST_AUTO_TEST_SUITE_END()