#pragma once

// system includes ---------------------------------------------------------
#include <malloc.h>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <cassert>
#include <complex>
#include <type_traits>
#include <vector>
// own includes ------------------------------------------------------------
#include "base/types.hpp"
//#include "fft/fft2.hpp"
#include "fft/fft2_r2c.hpp"
#include "fft/ft_grid_helpers.hpp"
// local includes ----------------------------------------------------------
#include "fold.hpp"
#include "init_fftw.hpp"
#include "lambda.hpp"
#include "ridgelet_cell_array.hpp"

namespace internal {

/**
 * @brief dst = src1*src2
 *
 * @param[out] dst
 * @param[in] src1
 * @param[in] src2
 */
template <typename ARRAY, typename DERIVED>
void
mtimes(ARRAY &dst,
       const Eigen::SparseMatrix<double, Eigen::RowMajor> &src1,
       const Eigen::DenseBase<DERIVED> &src2)
{
  typedef Eigen::SparseMatrix<double, Eigen::RowMajor> sp_mat_t;

  assert(src1.rows() == src2.rows());
  assert(src1.cols() == src2.cols());
  assert(dst.rows() == src1.rows());
  assert(dst.cols() == src1.cols());

  dst.setZero();

  for (int k = 0; k < src1.outerSize(); ++k) {
    for (typename sp_mat_t::InnerIterator it(src1, k); it; ++it) {
      int row = it.row();
      int col = it.col();
      double val = it.value();
      dst(row, col) = val * src2(row, col);
    }
  }
}
}  // internal

// forward declaration
class RidgeletFrame;

/**
 * @brief Fast Fourier Ridgelet Transform
 *
 */
template <typename NUMERIC_T = std::complex<double>,
          typename FRAME = RidgeletFrame,
          typename FFT_TYPE = FFTr2c<PlannerR2COD>>
class RT
{
 public:
  typedef Eigen::Array<std::complex<double>, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
      complex_array_t;
  typedef Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> array_t;
  typedef Eigen::Array<NUMERIC_T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> rt_coeff_t;
  typedef FRAME rt_frame_t;
  typedef FFT_TYPE fft_t;
  typedef NUMERIC_T numeric_t;

 public:
  RT(const rt_frame_t &rt_frame)
      : rt_frame_(rt_frame)
  { /* empty */
  }

  RT(const RT<NUMERIC_T, FRAME, FFT_TYPE> &other)
      : rt_frame_(other.rt_frame)
  { /*  empty  */
  }

  RT(RT<NUMERIC_T, FRAME, FFT_TYPE> &&other)
      : rt_frame_(std::move(other.rt_frame))
  { /*  empty  */
  }

  RT<NUMERIC_T, FRAME, FFT_TYPE> &operator=(RT<NUMERIC_T, FRAME, FFT_TYPE> &&other);

  RT<NUMERIC_T, FRAME, FFT_TYPE> &operator=(const RT<NUMERIC_T, FRAME, FFT_TYPE> &other);

  RT() {}

  /**
   * @brief
   *
   * @param[out] dst  Ridgelet coefficients
   * @param[in]  src  Fourier coefficients in _centered zero-frequency_
   * convection
   */
  template <typename DERIVED>
  void rt(std::vector<rt_coeff_t> &dst, const Eigen::DenseBase<DERIVED> &src) const;

  template <typename DERIVED>
  void rt(RidgeletCellArray<rt_coeff_t> &dst, const Eigen::DenseBase<DERIVED> &src) const;

  /**
   * @brief
   *
   * @param[out] dst  Fourier coefficients in _centered zero-frequency_
   * convention
   * @param[in]  src  Ridgelet coefficients (real-valued)
   */
  template <typename DERIVED>
  void irt(Eigen::DenseBase<DERIVED> &dst, const std::vector<rt_coeff_t> &src) const;

  template <typename DERIVED>
  void irt(Eigen::DenseBase<DERIVED> &dst, const RidgeletCellArray<rt_coeff_t> &src) const;

  const rt_frame_t &frame() const { return rt_frame_; }

 private:
  rt_frame_t rt_frame_;
  // buffers
  thread_local static ArrayBuffer<> buf1_;
  thread_local static ArrayBuffer<> buf2_;
};

template <typename NUMERIC_T, typename FRAME, typename FFT_TYPE>
thread_local ArrayBuffer<> RT<NUMERIC_T, FRAME, FFT_TYPE>::buf1_;

template <typename NUMERIC_T, typename FRAME, typename FFT_TYPE>
thread_local ArrayBuffer<> RT<NUMERIC_T, FRAME, FFT_TYPE>::buf2_;

// --------------------------------------------------------------------------------
// --------------------------------------------------------------------------------
template <typename NUMERIC_T, typename FRAME, typename FFT_TYPE>
RT<NUMERIC_T, FRAME, FFT_TYPE> &
RT<NUMERIC_T, FRAME, FFT_TYPE>::operator=(RT<NUMERIC_T, FRAME, FFT_TYPE> &&other)
{
  rt_frame_ = std::move(other.rt_frame_);
}

template <typename NUMERIC_T, typename FRAME, typename FFT_TYPE>
RT<NUMERIC_T, FRAME, FFT_TYPE> &
RT<NUMERIC_T, FRAME, FFT_TYPE>::operator=(const RT<NUMERIC_T, FRAME, FFT_TYPE> &other)
{
  rt_frame_ = other.rt_frame_;
}

template <typename NUMERIC_T, typename FRAME, typename FFT_TYPE>
template <typename DERIVED>
void
RT<NUMERIC_T, FRAME, FFT_TYPE>::rt(std::vector<rt_coeff_t> &dst,
                                   const Eigen::DenseBase<DERIVED> &src) const
{
  static_assert(std::is_same<typename DERIVED::Scalar, complex_array_t::Scalar>::value,
                "type mismatch");
  const auto &lambdas = rt_frame_.lambdas();
  fft_t fft;

  const unsigned int rho_x = rt_frame_.rho_x();
  const unsigned int rho_y = rt_frame_.rho_y();

  for (int i = 0; i < lambdas.size(); ++i) {
    auto &lambda = lambdas[i];

    if (lambda.t == rt_type::S) {
      auto &RF = rt_frame_.get_dense(lambda);
      auto tmp = buf1_.get<complex_array_t>(RF.cols(), RF.rows());
      tmp = ftcut(src, RF.rows(), RF.cols()).array() * RF;
      fft.ift(dst[i], tmp);
    } else {
      // ridgelet coefficients are stored as sparse matrix
      auto &RF = rt_frame_.get_sparse(lambda);
      auto tmp = buf1_.get<complex_array_t>(RF.rows(), RF.cols());
      internal::mtimes(tmp, RF, ftcut(src, RF.rows(), RF.cols()));
      if (lambda.t == rt_type::X) {
        auto tmp2 = buf2_.get<complex_array_t>(8 * rho_y, tmp.cols());
        fold(tmp2, tmp, 8 * rho_y, 0);
        fft.ift(dst[i], tmp2);
      } else if (lambda.t == rt_type::Y) {
        auto tmp2 = buf2_.get<complex_array_t>(tmp.rows(), 8 * rho_x);
        fold(tmp2, tmp, 8 * rho_x, 1);
        fft.ift(dst[i], tmp2);
      } else if (lambda.t == rt_type::D) {
        auto tmp2 = buf2_.get<complex_array_t>(8 * rho_y, tmp.cols());
        fold(tmp2, tmp, 8 * rho_x, 0);
        fft.ift(dst[i], tmp2);
      } else {
        assert(false);
      }
    }
  }
}

// --------------------------------------------------------------------------------
template <typename NUMERIC_T, typename FRAME, typename FFT_TYPE>
template <typename DERIVED>
void
RT<NUMERIC_T, FRAME, FFT_TYPE>::rt(RidgeletCellArray<rt_coeff_t> &dst,
                                   const Eigen::DenseBase<DERIVED> &src) const
{
  this->rt(dst.coeffs(), src);
}
namespace local_ {

template <typename DERIVED, typename T>
void
add_to_dst(Eigen::DenseBase<DERIVED> &dst, const Eigen::Map<T> &out)
{
  int right_nrows = out.rows();
  int right_ncols = out.cols();
  int nrows = dst.rows();
  int ncols = dst.cols();
  int row_offset = nrows / 2 - right_nrows / 2;
  int col_offset = ncols / 2 - right_ncols / 2;
  assert(row_offset >= 0);
  assert(col_offset >= 0);
  dst.block(row_offset, col_offset, right_nrows, right_ncols) += out;
}

}  // local_
// --------------------------------------------------------------------------------
template <typename NUMERIC_T, typename FRAME, typename FFT_TYPE>
template <typename DERIVED>
void
RT<NUMERIC_T, FRAME, FFT_TYPE>::irt(Eigen::DenseBase<DERIVED> &dst,
                                    const std::vector<rt_coeff_t> &src) const
{
  static_assert(std::is_same<typename DERIVED::Scalar, complex_array_t::Scalar>::value,
                "type mismatch");

  assert(dst.rows() == rt_frame_.Ny());
  assert(dst.cols() == rt_frame_.Nx());

  const auto &lambdas = rt_frame_.lambdas();

  dst.setZero();
  fft_t fft;

  for (int i = 0; i < lambdas.size(); ++i) {
    auto &lambda = lambdas[i];
    const bool ft_scale = false;
    auto f_tilde_hat = buf1_.get<complex_array_t>(src[i].rows(), src[i].cols());
    fft.ft(f_tilde_hat, src[i], ft_scale);
    if (lambda.t == rt_type::S) {
      auto &ridge = rt_frame_.get_dense(lambda);
      auto out = buf2_.get<complex_array_t>(ridge.rows(), ridge.cols());
      out = f_tilde_hat * ridge;
      local_::add_to_dst(dst.derived(), out);
    } else if (lambda.t == rt_type::X) {
      auto &ridge = rt_frame_.get_sparse(lambda);
      auto tmp = buf2_.get<complex_array_t>(ridge.rows(), ridge.cols());
      unfold(tmp, f_tilde_hat, ridge.rows(), 0);
      auto out = buf1_.get<complex_array_t>(ridge.rows(), ridge.cols());
      internal::mtimes(out, ridge, tmp);
      local_::add_to_dst(dst.derived(), out);
    } else if (lambda.t == rt_type::Y) {
      auto &ridge = rt_frame_.get_sparse(lambda);
      auto tmp = buf2_.get<complex_array_t>(ridge.rows(), ridge.cols());
      unfold(tmp, f_tilde_hat, ridge.cols(), 1);
      auto out = buf1_.get<complex_array_t>(ridge.rows(), ridge.cols());
      internal::mtimes(out, ridge, tmp);
      local_::add_to_dst(dst.derived(), out);
    } else if (lambda.t == rt_type::D) {
      auto &ridge = rt_frame_.get_sparse(lambda);
      auto tmp = buf2_.get<complex_array_t>(ridge.rows(), ridge.cols());
      unfold(tmp, f_tilde_hat, ridge.rows(), 0);
      auto out = buf1_.get<complex_array_t>(ridge.rows(), ridge.cols());
      internal::mtimes(out, ridge, tmp);
      local_::add_to_dst(dst.derived(), out);
    }
  }
}

// --------------------------------------------------------------------------------
template <typename NUMERIC_T, typename FRAME, typename FFT_TYPE>
template <typename DERIVED>
void
RT<NUMERIC_T, FRAME, FFT_TYPE>::irt(Eigen::DenseBase<DERIVED> &dst,
                                    const RidgeletCellArray<rt_coeff_t> &src) const
{
  this->irt(dst, src.coeffs());
}


#include "b18TrafficJohnson.h"

#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/graph/exterior_property.hpp>
#include <iostream>
#include <fstream>
using namespace std;
#include "src/linux_host_memory_logger.h"

#define ROUTE_DEBUG 0
//#define DEBUG_JOHNSON 0

namespace LC {

////////////////
/////////////////////////////
using namespace boost;

inline bool fileExists(const std::string& fileName) {
  std::ifstream f(fileName.c_str());
  return f.good();
}

typedef exterior_vertex_property<RoadGraph::roadBGLGraph_BI, float>
DistanceProperty;
typedef DistanceProperty::matrix_type DistanceMatrix;
typedef DistanceProperty::matrix_map_type DistanceMatrixMap;

void B18TrafficJohnson::generateRoutes(
    LC::RoadGraph::roadBGLGraph_BI &roadGraph,
    std::vector<B18TrafficPerson> &trafficPersonVec,
    std::vector<uint>& indexPathVec,
    std::map<RoadGraph::roadGraphEdgeDesc_BI, uint> &edgeDescToLaneMapNum,
    int weigthMode, float sample) {
  if (trafficPersonVec.empty()) {
    printf("ERROR generateRoutes: people empty");
    return;
  }

  printf(">> generatePathRoutes\n");
  QTime timer;
  timer.start();

  uint currIndexPath = 0;
  std::vector<uint> oldIndexPathVec = indexPathVec; // std::move(indexPathVec); // avoid copying
  indexPathVec.clear(); // = std::vector<uint>();

  // 1. Update weight edges
  printf(">> generateRoutes Update weight edges\n");
  int numEdges = 0;
  property_map<RoadGraph::roadBGLGraph_BI, float RoadGraphEdge::*>::type
    weight_pmap = boost::get(&RoadGraphEdge::edge_weight, roadGraph);

  RoadGraph::roadGraphEdgeIter_BI ei, eiEnd;
  float minTravelTime = FLT_MAX;
  float maxTravelTime = -FLT_MAX;
  float minLength = FLT_MAX;
  float maxLength = -FLT_MAX;
  float minSpeed = FLT_MAX;
  float maxSpeed = -FLT_MAX;
  for (boost::tie(ei, eiEnd) = boost::edges(roadGraph); ei != eiEnd; ++ei) {
    numEdges++;
    if ((edgeDescToLaneMapNum.size() > 20) && (numEdges % (edgeDescToLaneMapNum.size() / 20)) == 0) {
      printf("Edge %d of %d (%2.0f%%)\n", numEdges, edgeDescToLaneMapNum.size(), (100.0f * numEdges) / edgeDescToLaneMapNum.size());
    }
    if (roadGraph[*ei].numberOfLanes > 0) {
      float speed;
      if (weigthMode == 0 || roadGraph[*ei].averageSpeed.size() <= 0) {
        speed = roadGraph[*ei].maxSpeedMperSec;//(p0-p1).length();

      } else {
        // Use the avarage speed sampled in a former step
        float avOfAv = 0;
        for (int avOfAvN = 0; avOfAvN < roadGraph[*ei].averageSpeed.size(); avOfAvN++) {
          avOfAv += roadGraph[*ei].averageSpeed[avOfAvN];
        }
        speed = avOfAv / roadGraph[*ei].averageSpeed.size();
      }
      float travelTime = roadGraph[*ei].edgeLength / speed;
      roadGraph[*ei].edge_weight = travelTime;
      weight_pmap[*ei] = travelTime;

      minTravelTime = minTravelTime>travelTime ? travelTime : minTravelTime;
      maxTravelTime = maxTravelTime < travelTime ? travelTime : maxTravelTime;

      minLength = minLength > roadGraph[*ei].edgeLength ? roadGraph[*ei].edgeLength : minLength;
      maxLength = maxLength < roadGraph[*ei].edgeLength ? roadGraph[*ei].edgeLength : maxLength;

      minSpeed = minSpeed > speed ? speed : minSpeed;
      maxSpeed = maxSpeed < speed ? speed : maxSpeed;
    } else {
      roadGraph[*ei].edge_weight =
        100000000.0; //FLT_MAX;// if it has not lines, then the weight is inf
    }
  }
  printf("Travel time Min: %f Max: %f\n", minTravelTime, maxTravelTime);
  printf("Length Min: %f Max: %f\n", minLength, maxLength);
  printf("Speed Min: %f Max: %f\n", minSpeed, maxSpeed);

  //2. Generate route for each person
  printf(">> generateRoutesGenerate\n");
  int numVertex = boost::num_vertices(roadGraph);
  //vertex
  typedef LC::RoadGraph::roadGraphVertexDesc_BI VertexDescriptor;
  typedef std::map<VertexDescriptor, size_t> VertexIndexMap;
  VertexIndexMap mapVertexIndex;
  boost::associative_property_map<VertexIndexMap> pmVertexIndex(mapVertexIndex);

  DistanceMatrix distances(numVertex);
  DistanceMatrixMap dm(distances, roadGraph);

  ////////////////////////
  // CALL JOHNSON
  //const bool tryReadWriteFirstJohnsonArray = false;
  const bool tryReadWriteFirstJohnsonArray = weigthMode == 0;
  std::string fileName = "johnson_numVertex_" + std::to_string(numVertex) + "_maxTravelTime_" + std::to_string(maxTravelTime) + ".bin"; // encode num vertext and travel time to "check" is the same input
  bool johnsonReadCorrectly = false;
  if (tryReadWriteFirstJohnsonArray && fileExists(fileName)) {
    printf("Loading Johnson...\n");
    // if exists try to read.
    std::ifstream in(fileName, std::ios::in | std::ios::binary);
    for (int vN = 0; vN < numVertex; vN++) {
      in.read((char *) &dm[vN][0], numVertex * sizeof(float));
    }
    printf("Johnson Loaded\n");
    johnsonReadCorrectly = true;
  }

  // Run Johnson since we could not find it or it is not the first iteration
  if (johnsonReadCorrectly == false) {
    printf("Call Johnson\n");
    memory_logger.ChangeMessageTo("Computing Johnson");
    boost::johnson_all_pairs_shortest_paths(roadGraph, dm, weight_map(weight_pmap));
    memory_logger.ChangeMessageTo("Storing Johnson");
    if (tryReadWriteFirstJohnsonArray) {
      // write to file
      printf("Johnson start writing...\n");
      std::ofstream out(fileName, std::ios::out | std::ios::binary);
      if (!out) {
        printf("ERROR: Tried to save %s but failed\n", fileName.c_str());
      } else {
        for (int vN = 0; vN < numVertex; vN++) {
          out.write((char *) &dm[vN][0], numVertex * sizeof(float));
        }
        printf("Johnson saved to file: %s\n", fileName.c_str());
      }
    }
  }
  memory_logger.ChangeMessageTo("End");

  #ifdef DEBUG_JOHNSON
  std::cerr << std::fixed << std::setprecision(2);
  for (int i = 0; i < numVertex; i++) {
    for (int j = 0; j < numVertex; j++) {
      std::cerr << " " << std::setw(10) << dm[i][j];
    }
    std::cerr << std::endl;
  }
  #endif

  printf("Create Johnson numVertex %d Time %d ms\n", numVertex, timer.elapsed());

  ////////////////////////
  // Create routes
  uint noAccesible = 0;
  uint sameSrcDst = 0;
  QTime timer2;
  timer2.start();
  const int kMaxNumPath = 250;

  for (int p = 0; p < trafficPersonVec.size(); p++) {
    if (trafficPersonVec.size() > 200) {
      if ((p % (trafficPersonVec.size() / 20)) == 0) {
        printf("Route %d of %d (%2.0f%%)\n", p, trafficPersonVec.size(), (100.0f * p) / trafficPersonVec.size());
      }
    }

    ////////////////////////////////////////////////////////////////////////////////////////////
    // Some people do not change route.
    if (sample != 1.0f) {
      if (sample > (((float) qrand()) / RAND_MAX)) { // not recalculate
        printf("Person %d does not change route\n", p);
        // Copy route directly
        uint oldIndex = trafficPersonVec[p].indexPathInit;
        trafficPersonVec[p].indexPathInit = currIndexPath;
        uint index = 0;
        while (oldIndexPathVec[oldIndex + index] != -1) {
          indexPathVec.push_back(oldIndexPathVec.at(oldIndex + index));
          currIndexPath++;
          index++;
        }
        indexPathVec.push_back(-1);
        currIndexPath++;
        continue;
      }
    }

    ////////////////////////////////////////////////////////////////////////////////////////////
    trafficPersonVec[p].indexPathInit = currIndexPath;

    LC::RoadGraph::roadGraphVertexDesc_BI srcvertex = trafficPersonVec[p].init_intersection;
    LC::RoadGraph::roadGraphVertexDesc_BI tgtvertex = trafficPersonVec[p].end_intersection;

    // check whether source same than target (we have arrived)
    if (tgtvertex == srcvertex) {
      //trafficPersonVec[p].indexPathInit = 0; // that index points to -1
      indexPathVec.push_back(-1);
      currIndexPath++;
      sameSrcDst++;
      continue;
    }

    // check if accesible
    if (dm[srcvertex][tgtvertex] == (std::numeric_limits < float >::max)()) {
      //trafficPersonVec[p].indexPathInit = 0; // that index points to -1
      indexPathVec.push_back(-1);
      currIndexPath++;
      noAccesible++;
      continue;
    }

    // find path
    LC::RoadGraph::roadGraphVertexDesc_BI currvertex = srcvertex;//init
    RoadGraph::out_roadGraphEdgeIter_BI Oei, Oei_end;
    LC::RoadGraph::roadGraphVertexDesc_BI srcPosEdgeV, tgtPosEdgeV;

    int currIndex = 0;

    while (currvertex != tgtvertex) {
      // check all outedges from currvertex which one continues to shortest path
      bool cont = false;

      for (boost::tie(Oei, Oei_end) = boost::out_edges(currvertex, roadGraph); Oei != Oei_end; ++Oei) {
        srcPosEdgeV = boost::source(*Oei, roadGraph);
        tgtPosEdgeV = boost::target(*Oei, roadGraph);

        if (std::abs<float>(dm[currvertex][tgtPosEdgeV] + dm[tgtPosEdgeV][tgtvertex] - dm[currvertex][tgtvertex]) < 0.1f) {  // link found
          std::pair<RoadGraph::roadGraphEdgeDesc_BI, bool> edge_pair = boost::edge(srcPosEdgeV, tgtPosEdgeV, roadGraph);

          if (edge_pair.second == false) {
            printf("****edge not found\n");//this should not happen
            currvertex = tgtPosEdgeV;
            break;//break for
          } else {
            if (edgeDescToLaneMapNum.find(edge_pair.first) == edgeDescToLaneMapNum.end()) {
              printf("****Unknown edge\n");//this should not happen
              currvertex = tgtvertex;//end loop
              break;//break for
            }

            uint lane = edgeDescToLaneMapNum[edge_pair.first];
            indexPathVec.push_back(lane);
            currIndexPath++;
            currIndex++;  // this person number jumps.

            if (currIndex >= kMaxNumPath - 1) {  // This is very uncommon (just finish person).
              // printf("currIndex > 250\n");
              currvertex = tgtvertex;//end loop
              break;//break for
            }

            currvertex = tgtPosEdgeV;
            cont = true;
            break;//break for
          }
        }
      }

      if (cont == true) {
        continue;
      }

      // not foudn edge
      //printf("****none edge works or > 250\n");  //this should not happen
      //exit(0);//!!! REMOVE
      break;
    }//while find tgt

    indexPathVec.push_back(-1);
    currIndexPath++;
    ////////////////////////////////////////////////////////////////////////////////////////////
  }
 printf("Final Path Size %u\n", currIndexPath);
  for (int p = 0; p < trafficPersonVec.size(); p++) {
    trafficPersonVec[p].indexPathCurr = trafficPersonVec[p].indexPathInit;
  }

  std::cerr
    << "Finished with Johnson routing:" << std::endl
    << "- No accesible ODs: " << noAccesible << std::endl
    << "- Sames src dst ODs: " << sameSrcDst << std::endl
    << "- Shortest path length (distance -> amount of ODs): " << std::endl;

  std::vector<int> amountOfEdges(300, 0);
  for (const auto p : trafficPersonVec) {
    int d = 0;
    int cur = p.indexPathInit;
    while (indexPathVec.at(cur) != -1) { cur++; d++; }
    amountOfEdges.at(d)++;
  }
  for (int i = 0; i < 300; i++) {
    if (amountOfEdges.at(i) != 0)
      std::cerr << '\t' << i << " -> " << amountOfEdges.at(i) << std::endl;
  }

  #ifdef DEBUG_JOHNSON
  std::cerr << "indexPathVec: " << std::endl;
  int i = 0;
  for (const auto x : indexPathVec) {
    std::cerr << i++ << " " << x << " " << std::endl;
  }
  std::cerr << "trafficPersonVec: " << std::endl;
  for (const auto p : trafficPersonVec) {
    std::cerr << p.indexPathInit << " " << p.indexPathCurr << std::endl;
  }
  #endif

}

}  // Closing namespace LC


/*

Copyright (c) 2005-2022, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef ABSTRACTBACKWARDEULERCARDIACCELL_HPP_
#define ABSTRACTBACKWARDEULERCARDIACCELL_HPP_

#include <cassert>
#include <cmath>

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>
#include "ClassIsAbstract.hpp"

#include "AbstractCardiacCell.hpp"
#include "Exception.hpp"
#include "PetscTools.hpp"
#include "TimeStepper.hpp"

/**
 * This is the base class for cardiac cells solved using a (decoupled) backward
 * Euler approach (see http://dx.doi.org/10.1109/TBME.2006.879425).
 *
 * The basic approach to solving such models is:
 *  \li Update the transmembrane potential, either from solving an external PDE,
 *      or using a forward Euler step.
 *  \li Update any gating variables (or similar) using a backward euler step.
 *      Suitable ODEs can be written in the form  \f$du/dt = g(V) + h(V)*u\f$.  The update
 *      expression is then \f$u_n = ( u_{n-1} + g(V_n)*dt ) / ( 1 - h(V_n)*dt )\f$.
 *  \li Update the remaining state variables using Newton's method to solve the
 *      nonlinear system \f$U_n - U_{n-1} = dt*F(U_n, V_n)\f$.
 *      The template parameter to the class specifies the size of this nonlinear system.
 */
template<unsigned SIZE>
class AbstractBackwardEulerCardiacCell : public AbstractCardiacCell
{
    private:
    /** Needed for serialization. */
    friend class boost::serialization::access;
    /**
     * Archive the member variables.
     *
     * @param archive
     * @param version
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        // This calls serialize on the base class.
        archive & boost::serialization::base_object<AbstractCardiacCell>(*this);
    }
public:

    /**
     * Standard constructor for a cell.
     *
     * @param numberOfStateVariables  the size of the ODE system
     * @param voltageIndex  the index of the variable representing the transmembrane
     *     potential within the state variable vector
     * @param pIntracellularStimulus  the intracellular stimulus function
     *
     * Some notes for future reference:
     *  \li We may want to remove the timestep from this class, and instead pass it to
     *      the Compute* methods, especially if variable timestepping is to be used.
     *  \li It's a pity that inheriting from AbstractCardiacCell forces us to store a
     *      null pointer (for the unused ODE solver) in every instance.  We may want
     *      to revisit this design decision at a later date.
     */
    AbstractBackwardEulerCardiacCell(
        unsigned numberOfStateVariables,
        unsigned voltageIndex,
        boost::shared_ptr<AbstractStimulusFunction> pIntracellularStimulus);

    /** Virtual destructor */
    virtual ~AbstractBackwardEulerCardiacCell();

    /**
     * Compute the residual of the nonlinear system portion of the cell model.
     *
     * @param time  the current time
     * @param rCurrentGuess  the current guess for \f$U_n\f$
     * @param rResidual  to be filled in with the residual vector
     */
    virtual void ComputeResidual(double time, const double rCurrentGuess[SIZE], double rResidual[SIZE])=0;

    /**
     * Compute the Jacobian matrix for the nonlinear system portion of the cell model.
     *
     * @param time  the current time
     * @param rCurrentGuess  the current guess for \f$U_n\f$
     * @param rJacobian  to be filled in with the Jacobian matrix
     */
    virtual void ComputeJacobian(double time, const double rCurrentGuess[SIZE], double rJacobian[SIZE][SIZE])=0;

    /**
     * Simulates this cell's behaviour between the time interval [tStart, tEnd],
     * with timestep #mDt.  Uses a forward Euler step to update the transmembrane
     * potential at each timestep.
     *
     * The length of the time interval must be a multiple of the timestep.
     *
     * @param tStart  beginning of the time interval to simulate
     * @param tEnd  end of the time interval to simulate
     * @param tSamp  sampling interval for returned results (defaults to #mDt)
     * @return  the values of each state variable, at intervals of tSamp.
     */
    OdeSolution Compute(double tStart, double tEnd, double tSamp=0.0);

    /**
     * Simulates this cell's behaviour between the time interval [tStart, tEnd],
     * with timestep #mDt.  The transmembrane potential is kept fixed throughout.
     *
     * The length of the time interval must be a multiple of the timestep.
     *
     * @param tStart  beginning of the time interval to simulate
     * @param tEnd  end of the time interval to simulate
     */
    void ComputeExceptVoltage(double tStart, double tEnd);

    /**
     * Simulate this cell's behaviour between the time interval [tStart, tEnd],
     * with timestemp #mDt, updating the internal state variable values.
     *
     * @param tStart  beginning of the time interval to simulate
     * @param tEnd  end of the time interval to simulate
     */
    void SolveAndUpdateState(double tStart, double tEnd);

private:
// LCOV_EXCL_START
    /**
     * This function should never be called - the cell class incorporates its own solver.
     *
     * @param time
     * @param rY
     * @param rDY
     */
    void EvaluateYDerivatives(double time, const std::vector<double> &rY, std::vector<double> &rDY)
    {
        NEVER_REACHED;
    }
// LCOV_EXCL_STOP

protected:
    /**
     * Compute the values of all state variables, except the voltage, using backward Euler,
     * for one timestep from tStart.
     *
     * \note This method must be provided by subclasses.
     *
     * @param tStart  start of this timestep
     */
    virtual void ComputeOneStepExceptVoltage(double tStart)=0;

    /**
     * Perform a forward Euler step to update the transmembrane potential.
     *
     * \note This method must be provided by subclasses.
     *
     * @param time  start of this timestep
     */
    virtual void UpdateTransmembranePotential(double time)=0;
};

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
/*
 * NOTE: Explicit instantiation is not used for this class, because the SIZE
 * template parameter could take arbitrary values.
 */

template <unsigned SIZE>
AbstractBackwardEulerCardiacCell<SIZE>::AbstractBackwardEulerCardiacCell(
    unsigned numberOfStateVariables,
    unsigned voltageIndex,
    boost::shared_ptr<AbstractStimulusFunction> pIntracellularStimulus)
        : AbstractCardiacCell(boost::shared_ptr<AbstractIvpOdeSolver>(),
                              numberOfStateVariables,
                              voltageIndex,
                              pIntracellularStimulus)
{}

template <unsigned SIZE>
AbstractBackwardEulerCardiacCell<SIZE>::~AbstractBackwardEulerCardiacCell()
{}

template <unsigned SIZE>
OdeSolution AbstractBackwardEulerCardiacCell<SIZE>::Compute(double tStart, double tEnd, double tSamp)
{
    // In this method, we iterate over timesteps, doing the following for each:
    //   - update V using a forward Euler step
    //   - call ComputeExceptVoltage(t) to update the remaining state variables
    //     using backward Euler

    // Check length of time interval
    if (tSamp < mDt)
    {
        tSamp = mDt;
    }
    double _n_steps = (tEnd - tStart) / tSamp;
    const unsigned n_steps = (unsigned) floor(_n_steps+0.5);
    assert(fabs(tStart+n_steps*tSamp - tEnd) < 1e-12);
    const unsigned n_small_steps = (unsigned) floor(tSamp/mDt+0.5);
    assert(fabs(mDt*n_small_steps - tSamp) < 1e-12);

    // Initialise solution store
    OdeSolution solutions;
    solutions.SetNumberOfTimeSteps(n_steps);
    solutions.rGetSolutions().push_back(rGetStateVariables());
    solutions.rGetTimes().push_back(tStart);
    solutions.SetOdeSystemInformation(this->mpSystemInfo);

    // Loop over time
    double curr_time = tStart;
    for (unsigned i=0; i<n_steps; i++)
    {
        for (unsigned j=0; j<n_small_steps; j++)
        {
            curr_time = tStart + i*tSamp + j*mDt;

            // Compute next value of V
            UpdateTransmembranePotential(curr_time);

            // Compute other state variables
            ComputeOneStepExceptVoltage(curr_time);

            // check gating variables are still in range
            VerifyStateVariables();
        }

        // Update solutions
        solutions.rGetSolutions().push_back(rGetStateVariables());
        solutions.rGetTimes().push_back(curr_time+mDt);
    }

    return solutions;
}

template <unsigned SIZE>
void AbstractBackwardEulerCardiacCell<SIZE>::ComputeExceptVoltage(double tStart, double tEnd)
{
    // This method iterates over timesteps, calling ComputeExceptVoltage(t) at
    // each one, to update all state variables except for V, using backward Euler.

    // Check length of time interval
    unsigned n_steps = (unsigned)((tEnd - tStart) / mDt + 0.5);
    assert(fabs(tStart + n_steps*mDt - tEnd) < 1e-12);

    // Loop over time
    double curr_time;
    for (unsigned i=0; i<n_steps; i++)
    {
        curr_time = tStart + i*mDt;

        // Compute other state variables
        ComputeOneStepExceptVoltage(curr_time);

#ifndef NDEBUG
        // Check gating variables are still in range
        VerifyStateVariables();
#endif // NDEBUG
    }
}

template<unsigned SIZE>
void AbstractBackwardEulerCardiacCell<SIZE>::SolveAndUpdateState(double tStart, double tEnd)
{
    TimeStepper stepper(tStart, tEnd, mDt);

    while (!stepper.IsTimeAtEnd())
    {
        double time = stepper.GetTime();

        // Compute next value of V
        UpdateTransmembranePotential(time);

        // Compute other state variables
        ComputeOneStepExceptVoltage(time);

        // check gating variables are still in range
        VerifyStateVariables();

        stepper.AdvanceOneTimeStep();
    }
}

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

/**
 * Specialization for the case where there are no non-linear ODEs in the model.
 */
template<>
class AbstractBackwardEulerCardiacCell<0u> : public AbstractCardiacCell
{
private:
    /** Needed for serialization. */
    friend class boost::serialization::access;
    /**
     * Archive the member variables.
     *
     * @param archive
     * @param version
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        // This calls serialize on the base class.
        archive & boost::serialization::base_object<AbstractCardiacCell>(*this);
    }

public:
    /**
     * Standard constructor for a cell.
     *
     * @param numberOfStateVariables  the size of the ODE system
     * @param voltageIndex  the index of the variable representing the transmembrane
     *     potential within the state variable vector
     * @param pIntracellularStimulus  the intracellular stimulus function
     */
    AbstractBackwardEulerCardiacCell(unsigned numberOfStateVariables,
                                     unsigned voltageIndex,
                                     boost::shared_ptr<AbstractStimulusFunction> pIntracellularStimulus)
        : AbstractCardiacCell(boost::shared_ptr<AbstractIvpOdeSolver>(),
                              numberOfStateVariables,
                              voltageIndex,
                              pIntracellularStimulus)
    {}

    /** Virtual destructor */
    virtual ~AbstractBackwardEulerCardiacCell()
    {}

    /**
     * Simulates this cell's behaviour between the time interval [tStart, tEnd],
     * with timestep #mDt.  Uses a forward Euler step to update the transmembrane
     * potential at each timestep.
     *
     * The length of the time interval must be a multiple of the timestep.
     *
     * @param tStart  beginning of the time interval to simulate
     * @param tEnd  end of the time interval to simulate
     * @param tSamp  sampling interval for returned results (defaults to #mDt)
     * @return  the values of each state variable, at intervals of tSamp.
     */
    OdeSolution Compute(double tStart, double tEnd, double tSamp=0.0)
    {
        // In this method, we iterate over timesteps, doing the following for each:
        //   - update V using a forward Euler step
        //   - call ComputeExceptVoltage(t) to update the remaining state variables
        //     using backward Euler

        // Check length of time interval
        if (tSamp < mDt)
        {
            tSamp = mDt;
        }
        double _n_steps = (tEnd - tStart) / tSamp;
        const unsigned n_steps = (unsigned) floor(_n_steps+0.5);
        assert(fabs(tStart+n_steps*tSamp - tEnd) < 1e-12);
        const unsigned n_small_steps = (unsigned) floor(tSamp/mDt+0.5);
        assert(fabs(mDt*n_small_steps - tSamp) < 1e-12);

        // Initialise solution store
        OdeSolution solutions;
        solutions.SetNumberOfTimeSteps(n_steps);
        solutions.rGetSolutions().push_back(rGetStateVariables());
        solutions.rGetTimes().push_back(tStart);
        solutions.SetOdeSystemInformation(this->mpSystemInfo);

        // Loop over time
        double curr_time = tStart;
        for (unsigned i=0; i<n_steps; i++)
        {
            for (unsigned j=0; j<n_small_steps; j++)
            {
                curr_time = tStart + i*tSamp + j*mDt;

                // Compute next value of V
                UpdateTransmembranePotential(curr_time);

                // Compute other state variables
                ComputeOneStepExceptVoltage(curr_time);

                // check gating variables are still in range
                VerifyStateVariables();
            }

            // Update solutions
            solutions.rGetSolutions().push_back(rGetStateVariables());
            solutions.rGetTimes().push_back(curr_time+mDt);
        }

        return solutions;
    }

    /**
     * Simulates this cell's behaviour between the time interval [tStart, tEnd],
     * with timestep #mDt.  The transmembrane potential is kept fixed throughout.
     *
     * The length of the time interval must be a multiple of the timestep.
     *
     * @param tStart  beginning of the time interval to simulate
     * @param tEnd  end of the time interval to simulate
     */
    void ComputeExceptVoltage(double tStart, double tEnd)
    {
        // This method iterates over timesteps, calling ComputeExceptVoltage(t) at
        // each one, to update all state variables except for V, using backward Euler.

        // Check length of time interval
        unsigned n_steps = (unsigned)((tEnd - tStart) / mDt + 0.5);
        assert(fabs(tStart + n_steps*mDt - tEnd) < 1e-12);

        // Loop over time
        double curr_time;
        for (unsigned i=0; i<n_steps; i++)
        {
            curr_time = tStart + i*mDt;

            // Compute other state variables
            ComputeOneStepExceptVoltage(curr_time);

#ifndef NDEBUG
            // Check gating variables are still in range
            VerifyStateVariables();
#endif // NDEBUG
        }
    }

    /**
     * Simulate this cell's behaviour between the time interval [tStart, tEnd],
     * with timestemp #mDt, updating the internal state variable values.
     *
     * @param tStart  beginning of the time interval to simulate
     * @param tEnd  end of the time interval to simulate
     */
    void SolveAndUpdateState(double tStart, double tEnd)
    {
        TimeStepper stepper(tStart, tEnd, mDt);

        while (!stepper.IsTimeAtEnd())
        {
            double time = stepper.GetTime();

            // Compute next value of V
            UpdateTransmembranePotential(time);

            // Compute other state variables
            ComputeOneStepExceptVoltage(time);

            // Check gating variables are still in range
            VerifyStateVariables();

            stepper.AdvanceOneTimeStep();
        }
    }

private:
// LCOV_EXCL_START
    /**
     * This function should never be called - the cell class incorporates its own solver.
     *
     * @param time
     * @param rY
     * @param rDY
     */
    void EvaluateYDerivatives(double time, const std::vector<double> &rY, std::vector<double> &rDY)
    {
        NEVER_REACHED;
    }
// LCOV_EXCL_STOP

protected:
    /**
     * Compute the values of all state variables, except the voltage, using backward Euler,
     * for one timestep from tStart.
     *
     * \note This method must be provided by subclasses.
     *
     * @param tStart  start of this timestep
     */
    virtual void ComputeOneStepExceptVoltage(double tStart)=0;

    /**
     * Perform a forward Euler step to update the transmembrane potential.
     *
     * \note This method must be provided by subclasses.
     *
     * @param time  start of this timestep
     */
    virtual void UpdateTransmembranePotential(double time)=0;
};

// Debugging
#ifndef NDEBUG
#include "OutputFileHandler.hpp"
#include <boost/foreach.hpp>
template<unsigned SIZE>
void DumpJacobianToFile(double time, const double rCurrentGuess[SIZE], double rJacobian[SIZE][SIZE],
                        const std::vector<double>& rY)
{
    OutputFileHandler handler("DumpJacs", false);
    out_stream p_file = handler.OpenOutputFile("J.txt", std::ios::app);
    (*p_file) << "At " << time << " " << SIZE << std::endl;
    (*p_file) << "rY";
    BOOST_FOREACH(double y, rY)
    {
        (*p_file) << " " << y;
    }
    (*p_file) << std::endl;
    (*p_file) << "rCurrentGuess";
    for (unsigned i=0; i<SIZE; i++)
    {
        (*p_file) << " " << rCurrentGuess[i];
    }
    (*p_file) << std::endl;
    (*p_file) << "rJacobian";
    for (unsigned i=0; i<SIZE; i++)
    {
        for (unsigned j=0; j<SIZE; j++)
        {
            (*p_file) << " " << rJacobian[i][j];
        }
    }
    (*p_file) << std::endl;
}
#endif

TEMPLATED_CLASS_IS_ABSTRACT_1_UNSIGNED(AbstractBackwardEulerCardiacCell)

#endif /*ABSTRACTBACKWARDEULERCARDIACCELL_HPP_*/


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


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