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

 *
 * Author: Timo Heister and Jiaqi Zhang, Clemson University, 2020
 */

// The first few files have already been covered in previous examples and will
// thus not be further commented on:
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/function_lib.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/sparse_direct.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/fe/mapping_q1.h>
// Here the discontinuous finite elements and FEInterfaceValues are defined.
#include <deal.II/fe/fe_dgq.h>
#include <deal.II/fe/fe_interface_values.h>

#include <deal.II/numerics/derivative_approximation.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/base/convergence_table.h>

#include <deal.II/meshworker/copy_data.h>
#include <deal.II/meshworker/mesh_loop.h>
#include <deal.II/meshworker/scratch_data.h>

namespace Step74
{
  using namespace dealii;

  // @sect3{Equation data}
  // Here we define two test cases: convergence_rate for a smooth function
  // and l_singularity for the Functions::LSingularityFunction.
  enum class TestCase
  {
    convergence_rate,
    l_singularity
  };

  // A smooth solution for the convergence test:
  template <int dim>
  class SmoothSolution : public Function<dim>
  {
  public:
    SmoothSolution()
      : Function<dim>()
    {}

    virtual void value_list(const std::vector<Point<dim>> &points,
                            std::vector<double> &          values,
                            const unsigned int component = 0) const override;

    virtual Tensor<1, dim>
    gradient(const Point<dim> & point,
             const unsigned int component = 0) const override;
  };

  template <int dim>
  void SmoothSolution<dim>::value_list(const std::vector<Point<dim>> &points,
                                       std::vector<double> &          values,
                                       const unsigned int /*component*/) const
  {
    using numbers::PI;
    for (unsigned int i = 0; i < values.size(); ++i)
      values[i] =
        std::sin(2. * PI * points[i][0]) * std::sin(2. * PI * points[i][1]);
  }

  template <int dim>
  Tensor<1, dim>
  SmoothSolution<dim>::gradient(const Point<dim> &point,
                                const unsigned int /*component*/) const
  {
    Tensor<1, dim> return_value;
    using numbers::PI;
    return_value[0] =
      2. * PI * std::cos(2. * PI * point[0]) * std::sin(2. * PI * point[1]);
    return_value[1] =
      2. * PI * std::sin(2. * PI * point[0]) * std::cos(2. * PI * point[1]);
    return return_value;
  }

  // The corresponding right-hand side of the smooth function:
  template <int dim>
  class SmoothRightHandSide : public Function<dim>
  {
  public:
    SmoothRightHandSide()
      : Function<dim>()
    {}

    virtual void value_list(const std::vector<Point<dim>> &points,
                            std::vector<double> &          values,
                            const unsigned int /*component*/) const override;
  };

  template <int dim>
  void
  SmoothRightHandSide<dim>::value_list(const std::vector<Point<dim>> &points,
                                       std::vector<double> &          values,
                                       const unsigned int /*component*/) const
  {
    using numbers::PI;
    for (unsigned int i = 0; i < values.size(); ++i)
      values[i] = 8. * PI * PI * std::sin(2. * PI * points[i][0]) *
                  std::sin(2. * PI * points[i][1]);
  }

  // The right-hand side that corresponds to the function
  // Functions::LSingularityFunction, where we
  // assume that the diffusion coefficient $\nu = 1$:
  template <int dim>
  class SingularRightHandSide : public Function<dim>
  {
  public:
    SingularRightHandSide()
      : Function<dim>()
    {}

    virtual void value_list(const std::vector<Point<dim>> &points,
                            std::vector<double> &          values,
                            const unsigned int /*component*/) const override;

  private:
    const Functions::LSingularityFunction ref;
  };

  template <int dim>
  void
  SingularRightHandSide<dim>::value_list(const std::vector<Point<dim>> &points,
                                         std::vector<double> &          values,
                                         const unsigned int /*component*/) const
  {
    for (unsigned int i = 0; i < values.size(); ++i)
      values[i] = -ref.laplacian(points[i]);
  }

  // @sect3{Auxiliary functions}
  // The following two auxiliary functions are used to compute
  // jump terms for $u_h$ and $\nabla u_h$ on a face,
  // respectively.
  template <int dim>
  void get_function_jump(const FEInterfaceValues<dim> &fe_iv,
                         const Vector<double> &        solution,
                         std::vector<double> &         jump)
  {
    const unsigned int                 n_q = fe_iv.n_quadrature_points;
    std::array<std::vector<double>, 2> face_values;
    jump.resize(n_q);
    for (unsigned int i = 0; i < 2; ++i)
      {
        face_values[i].resize(n_q);
        fe_iv.get_fe_face_values(i).get_function_values(solution,
                                                        face_values[i]);
      }
    for (unsigned int q = 0; q < n_q; ++q)
      jump[q] = face_values[0][q] - face_values[1][q];
  }

  template <int dim>
  void get_function_gradient_jump(const FEInterfaceValues<dim> &fe_iv,
                                  const Vector<double> &        solution,
                                  std::vector<Tensor<1, dim>> & gradient_jump)
  {
    const unsigned int          n_q = fe_iv.n_quadrature_points;
    std::vector<Tensor<1, dim>> face_gradients[2];
    gradient_jump.resize(n_q);
    for (unsigned int i = 0; i < 2; ++i)
      {
        face_gradients[i].resize(n_q);
        fe_iv.get_fe_face_values(i).get_function_gradients(solution,
                                                           face_gradients[i]);
      }
    for (unsigned int q = 0; q < n_q; ++q)
      gradient_jump[q] = face_gradients[0][q] - face_gradients[1][q];
  }

  // This function computes the penalty $\sigma$.
  double get_penalty_factor(const unsigned int fe_degree,
                            const double       cell_extent_left,
                            const double       cell_extent_right)
  {
    const unsigned int degree = std::max(1U, fe_degree);
    return degree * (degree + 1.) * 0.5 *
           (1. / cell_extent_left + 1. / cell_extent_right);
  }

  // @sect3{The CopyData}
  // In the following, we define "Copy" objects for the MeshWorker::mesh_loop(),
  // which is essentially the same as step-12. Note that the
  // "Scratch" object is not defined here because we use
  // MeshWorker::ScratchData<dim> instead. (The use of "Copy" and "Scratch"
  // objects is extensively explained in the WorkStream namespace documentation.
  struct CopyDataFace
  {
    FullMatrix<double>                   cell_matrix;
    std::vector<types::global_dof_index> joint_dof_indices;
    std::array<double, 2>                values;
    std::array<unsigned int, 2>          cell_indices;
  };

  struct CopyData
  {
    FullMatrix<double>                   cell_matrix;
    Vector<double>                       cell_rhs;
    std::vector<types::global_dof_index> local_dof_indices;
    std::vector<CopyDataFace>            face_data;
    double                               value;
    unsigned int                         cell_index;

    template <class Iterator>
    void reinit(const Iterator &cell, const unsigned int dofs_per_cell)
    {
      cell_matrix.reinit(dofs_per_cell, dofs_per_cell);
      cell_rhs.reinit(dofs_per_cell);
      local_dof_indices.resize(dofs_per_cell);
      cell->get_dof_indices(local_dof_indices);
    }
  };

  // @sect3{The SIPGLaplace class}
  // After these preparations, we proceed with the main class of this program,
  // called `SIPGLaplace`. The overall structure of the class is as in many
  // of the other tutorial programs. Major differences will only come up in the
  // implementation of the assemble functions, since we use FEInterfaceValues to
  // assemble face terms.
  template <int dim>
  class SIPGLaplace
  {
  public:
    SIPGLaplace(const TestCase &test_case);
    void run();

  private:
    void setup_system();
    void assemble_system();
    void solve();
    void refine_grid();
    void output_results(const unsigned int cycle) const;

    void   compute_errors();
    void   compute_error_estimate();
    double compute_energy_norm_error();

    Triangulation<dim>    triangulation;
    const unsigned int    degree;
    const QGauss<dim>     quadrature;
    const QGauss<dim - 1> face_quadrature;
    const QGauss<dim>     quadrature_overintegration;
    const QGauss<dim - 1> face_quadrature_overintegration;
    const MappingQ1<dim>  mapping;

    using ScratchData = MeshWorker::ScratchData<dim>;

    const FE_DGQ<dim> fe;
    DoFHandler<dim>   dof_handler;

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

    // The remainder of the class's members are used for the following:
    // - Vectors to store error estimator square and energy norm square per
    // cell.
    // - Print convergence rate and errors on the screen.
    // - The fiffusion coefficient $\nu$ is set to 1.
    // - Members that store information about the test case to be computed.
    Vector<double> estimated_error_square_per_cell;
    Vector<double> energy_norm_square_per_cell;

    ConvergenceTable convergence_table;

    const double diffusion_coefficient = 1.;

    const TestCase                       test_case;
    std::unique_ptr<const Function<dim>> exact_solution;
    std::unique_ptr<const Function<dim>> rhs_function;
  };

  // The constructor here takes the test case as input and then
  // determines the correct solution and right-hand side classes. The
  // remaining member variables are initialized in the obvious way.
  template <int dim>
  SIPGLaplace<dim>::SIPGLaplace(const TestCase &test_case)
    : degree(3)
    , quadrature(degree + 1)
    , face_quadrature(degree + 1)
    , quadrature_overintegration(degree + 2)
    , face_quadrature_overintegration(degree + 2)
    , mapping()
    , fe(degree)
    , dof_handler(triangulation)
    , test_case(test_case)
  {
    if (test_case == TestCase::convergence_rate)
      {
        exact_solution = std::make_unique<const SmoothSolution<dim>>();
        rhs_function   = std::make_unique<const SmoothRightHandSide<dim>>();
      }

    else if (test_case == TestCase::l_singularity)
      {
        exact_solution =
          std::make_unique<const Functions::LSingularityFunction>();
        rhs_function = std::make_unique<const SingularRightHandSide<dim>>();
      }
    else
      AssertThrow(false, ExcNotImplemented());
  }

  template <int dim>
  void SIPGLaplace<dim>::setup_system()
  {
    dof_handler.distribute_dofs(fe);
    DynamicSparsityPattern dsp(dof_handler.n_dofs());
    DoFTools::make_flux_sparsity_pattern(dof_handler, dsp);
    sparsity_pattern.copy_from(dsp);

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

  // @sect3{The assemble_system function}
  // The assemble function here is similar to that in step-12 and step-47.
  // Different from assembling by hand, we just need to focus
  // on assembling on each cell, each boundary face, and each
  // interior face. The loops over cells and faces are handled
  // automatically by MeshWorker::mesh_loop().
  //
  // The function starts by defining a local (lambda) function that is
  // used to integrate the cell terms:
  template <int dim>
  void SIPGLaplace<dim>::assemble_system()
  {
    const auto cell_worker =
      [&](const auto &cell, auto &scratch_data, auto &copy_data) {
        const FEValues<dim> &fe_v          = scratch_data.reinit(cell);
        const unsigned int   dofs_per_cell = fe_v.dofs_per_cell;
        copy_data.reinit(cell, dofs_per_cell);

        const auto &       q_points    = scratch_data.get_quadrature_points();
        const unsigned int n_q_points  = q_points.size();
        const std::vector<double> &JxW = scratch_data.get_JxW_values();

        std::vector<double> rhs(n_q_points);
        rhs_function->value_list(q_points, rhs);

        for (unsigned int point = 0; point < n_q_points; ++point)
          for (unsigned int i = 0; i < fe_v.dofs_per_cell; ++i)
            {
              for (unsigned int j = 0; j < fe_v.dofs_per_cell; ++j)
                copy_data.cell_matrix(i, j) +=
                  diffusion_coefficient *     // nu
                  fe_v.shape_grad(i, point) * // grad v_h
                  fe_v.shape_grad(j, point) * // grad u_h
                  JxW[point];                 // dx

              copy_data.cell_rhs(i) += fe_v.shape_value(i, point) * // v_h
                                       rhs[point] *                 // f
                                       JxW[point];                  // dx
            }
      };

    // Next, we need a function that assembles face integrals on the boundary:
    const auto boundary_worker = [&](const auto &        cell,
                                     const unsigned int &face_no,
                                     auto &              scratch_data,
                                     auto &              copy_data) {
      const FEFaceValuesBase<dim> &fe_fv = scratch_data.reinit(cell, face_no);

      const auto &       q_points      = scratch_data.get_quadrature_points();
      const unsigned int n_q_points    = q_points.size();
      const unsigned int dofs_per_cell = fe_fv.dofs_per_cell;

      const std::vector<double> &        JxW = scratch_data.get_JxW_values();
      const std::vector<Tensor<1, dim>> &normals =
        scratch_data.get_normal_vectors();

      std::vector<double> g(n_q_points);
      exact_solution->value_list(q_points, g);

      const double extent1 = cell->measure() / cell->face(face_no)->measure();
      const double penalty = get_penalty_factor(degree, extent1, extent1);

      for (unsigned int point = 0; point < n_q_points; ++point)
        {
          for (unsigned int i = 0; i < dofs_per_cell; ++i)
            for (unsigned int j = 0; j < dofs_per_cell; ++j)
              copy_data.cell_matrix(i, j) +=
                (-diffusion_coefficient *        // - nu
                   fe_fv.shape_value(i, point) * // v_h
                   (fe_fv.shape_grad(j, point) * // (grad u_h .
                    normals[point])              //  n)

                 - diffusion_coefficient *         // - nu
                     (fe_fv.shape_grad(i, point) * // (grad v_h .
                      normals[point]) *            //  n)
                     fe_fv.shape_value(j, point)   // u_h

                 + diffusion_coefficient * penalty * // + nu sigma
                     fe_fv.shape_value(i, point) *   // v_h
                     fe_fv.shape_value(j, point)     // u_h

                 ) *
                JxW[point]; // dx

          for (unsigned int i = 0; i < dofs_per_cell; ++i)
            copy_data.cell_rhs(i) +=
              (-diffusion_coefficient *        // - nu
                 (fe_fv.shape_grad(i, point) * // (grad v_h .
                  normals[point]) *            //  n)
                 g[point]                      // g

               + diffusion_coefficient * penalty *        // + nu sigma
                   fe_fv.shape_value(i, point) * g[point] // v_h g

               ) *
              JxW[point]; // dx
        }
    };

    // Finally, a function that assembles face integrals on interior
    // faces. To reinitialize FEInterfaceValues, we need to pass
    // cells, face and subface indices (for adaptive refinement) to
    // the reinit() function of FEInterfaceValues:
    const auto face_worker = [&](const auto &        cell,
                                 const unsigned int &f,
                                 const unsigned int &sf,
                                 const auto &        ncell,
                                 const unsigned int &nf,
                                 const unsigned int &nsf,
                                 auto &              scratch_data,
                                 auto &              copy_data) {
      const FEInterfaceValues<dim> &fe_iv =
        scratch_data.reinit(cell, f, sf, ncell, nf, nsf);

      const auto &       q_points   = fe_iv.get_quadrature_points();
      const unsigned int n_q_points = q_points.size();

      copy_data.face_data.emplace_back();
      CopyDataFace &     copy_data_face = copy_data.face_data.back();
      const unsigned int n_dofs_face    = fe_iv.n_current_interface_dofs();
      copy_data_face.joint_dof_indices  = fe_iv.get_interface_dof_indices();
      copy_data_face.cell_matrix.reinit(n_dofs_face, n_dofs_face);

      const std::vector<double> &        JxW     = fe_iv.get_JxW_values();
      const std::vector<Tensor<1, dim>> &normals = fe_iv.get_normal_vectors();

      const double extent1 = cell->measure() / cell->face(f)->measure();
      const double extent2 = ncell->measure() / ncell->face(nf)->measure();
      const double penalty = get_penalty_factor(degree, extent1, extent2);

      for (unsigned int point = 0; point < n_q_points; ++point)
        {
          for (unsigned int i = 0; i < n_dofs_face; ++i)
            for (unsigned int j = 0; j < n_dofs_face; ++j)
              copy_data_face.cell_matrix(i, j) +=
                (-diffusion_coefficient *              // - nu
                   fe_iv.jump(i, point) *              // [v_h]
                   (fe_iv.average_gradient(j, point) * // ({grad u_h} .
                    normals[point])                    //  n)

                 - diffusion_coefficient *               // - nu
                     (fe_iv.average_gradient(i, point) * // (grad v_h .
                      normals[point]) *                  //  n)
                     fe_iv.jump(j, point)                // [u_h]

                 + diffusion_coefficient * penalty * // + nu sigma
                     fe_iv.jump(i, point) *          // [v_h]
                     fe_iv.jump(j, point)            // [u_h]

                 ) *
                JxW[point]; // dx
        }
    };

    // The following lambda function will then copy data into the
    // global matrix and right-hand side.  Though there are no hanging
    // node constraints in DG discretization, we define an empty
    // AffineConstraints object that allows us to use the
    // AffineConstraints::distribute_local_to_global() functionality.
    AffineConstraints<double> constraints;
    constraints.close();
    const auto copier = [&](const auto &c) {
      constraints.distribute_local_to_global(c.cell_matrix,
                                             c.cell_rhs,
                                             c.local_dof_indices,
                                             system_matrix,
                                             system_rhs);

      // Copy data from interior face assembly to the global matrix.
      for (auto &cdf : c.face_data)
        {
          constraints.distribute_local_to_global(cdf.cell_matrix,
                                                 cdf.joint_dof_indices,
                                                 system_matrix);
        }
    };

    // With the assembly functions defined, we can now create
    // ScratchData and CopyData objects, and pass them together with
    // the lambda functions above to MeshWorker::mesh_loop(). In
    // addition, we need to specify that we want to assemble on
    // interior faces exactly once.
    const UpdateFlags cell_flags = update_values | update_gradients |
                                   update_quadrature_points | update_JxW_values;
    const UpdateFlags face_flags = update_values | update_gradients |
                                   update_quadrature_points |
                                   update_normal_vectors | update_JxW_values;

    ScratchData scratch_data(
      mapping, fe, quadrature, cell_flags, face_quadrature, face_flags);
    CopyData copy_data;

    MeshWorker::mesh_loop(dof_handler.begin_active(),
                          dof_handler.end(),
                          cell_worker,
                          copier,
                          scratch_data,
                          copy_data,
                          MeshWorker::assemble_own_cells |
                            MeshWorker::assemble_boundary_faces |
                            MeshWorker::assemble_own_interior_faces_once,
                          boundary_worker,
                          face_worker);
  }

  // @sect3{The solve() and output_results() function}
  // The following two functions are entirely standard and without difficulty.
  template <int dim>
  void SIPGLaplace<dim>::solve()
  {
    SparseDirectUMFPACK A_direct;
    A_direct.initialize(system_matrix);
    A_direct.vmult(solution, system_rhs);
  }

  template <int dim>
  void SIPGLaplace<dim>::output_results(const unsigned int cycle) const
  {
    const std::string filename = "sol_Q" + Utilities::int_to_string(degree, 1) +
                                 "-" + Utilities::int_to_string(cycle, 2) +
                                 ".vtu";
    std::ofstream output(filename);

    DataOut<dim> data_out;
    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution, "u", DataOut<dim>::type_dof_data);
    data_out.build_patches(mapping);
    data_out.write_vtu(output);
  }

  // @sect3{The compute_error_estimate() function}
  // The assembly of the error estimator here is quite similar to
  // that of the global matrix and right-had side and can be handled
  // by the MeshWorker::mesh_loop() framework. To understand what
  // each of the local (lambda) functions is doing, recall first that
  // the local cell residual is defined as
  // $h_K^2 \left\| f + \nu \Delta u_h \right\|_K^2$:
  template <int dim>
  void SIPGLaplace<dim>::compute_error_estimate()
  {
    const auto cell_worker =
      [&](const auto &cell, auto &scratch_data, auto &copy_data) {
        const FEValues<dim> &fe_v = scratch_data.reinit(cell);

        copy_data.cell_index = cell->active_cell_index();

        const auto &               q_points   = fe_v.get_quadrature_points();
        const unsigned int         n_q_points = q_points.size();
        const std::vector<double> &JxW        = fe_v.get_JxW_values();

        std::vector<Tensor<2, dim>> hessians(n_q_points);
        fe_v.get_function_hessians(solution, hessians);

        std::vector<double> rhs(n_q_points);
        rhs_function->value_list(q_points, rhs);

        const double hk                   = cell->diameter();
        double       residual_norm_square = 0;

        for (unsigned int point = 0; point < n_q_points; ++point)
          {
            const double residual =
              rhs[point] + diffusion_coefficient * trace(hessians[point]);
            residual_norm_square += residual * residual * JxW[point];
          }
        copy_data.value = hk * hk * residual_norm_square;
      };

    // Next compute boundary terms $\sum_{f\in \partial K \cap \partial \Omega}
    // \sigma \left\| [  u_h-g_D ]  \right\|_f^2  $:
    const auto boundary_worker = [&](const auto &        cell,
                                     const unsigned int &face_no,
                                     auto &              scratch_data,
                                     auto &              copy_data) {
      const FEFaceValuesBase<dim> &fe_fv = scratch_data.reinit(cell, face_no);

      const auto &   q_points   = fe_fv.get_quadrature_points();
      const unsigned n_q_points = q_points.size();

      const std::vector<double> &JxW = fe_fv.get_JxW_values();

      std::vector<double> g(n_q_points);
      exact_solution->value_list(q_points, g);

      std::vector<double> sol_u(n_q_points);
      fe_fv.get_function_values(solution, sol_u);

      const double extent1 = cell->measure() / cell->face(face_no)->measure();
      const double penalty = get_penalty_factor(degree, extent1, extent1);

      double difference_norm_square = 0.;
      for (unsigned int point = 0; point < q_points.size(); ++point)
        {
          const double diff = (g[point] - sol_u[point]);
          difference_norm_square += diff * diff * JxW[point];
        }
      copy_data.value += penalty * difference_norm_square;
    };

    // And finally interior face terms $\sum_{f\in \partial K}\lbrace \sigma
    // \left\| [u_h]  \right\|_f^2   +  h_f \left\|  [\nu \nabla u_h \cdot
    // \mathbf n ] \right\|_f^2 \rbrace$:
    const auto face_worker = [&](const auto &        cell,
                                 const unsigned int &f,
                                 const unsigned int &sf,
                                 const auto &        ncell,
                                 const unsigned int &nf,
                                 const unsigned int &nsf,
                                 auto &              scratch_data,
                                 auto &              copy_data) {
      const FEInterfaceValues<dim> &fe_iv =
        scratch_data.reinit(cell, f, sf, ncell, nf, nsf);

      copy_data.face_data.emplace_back();
      CopyDataFace &copy_data_face = copy_data.face_data.back();

      copy_data_face.cell_indices[0] = cell->active_cell_index();
      copy_data_face.cell_indices[1] = ncell->active_cell_index();

      const std::vector<double> &        JxW     = fe_iv.get_JxW_values();
      const std::vector<Tensor<1, dim>> &normals = fe_iv.get_normal_vectors();

      const auto &       q_points   = fe_iv.get_quadrature_points();
      const unsigned int n_q_points = q_points.size();

      std::vector<double> jump(n_q_points);
      get_function_jump(fe_iv, solution, jump);

      std::vector<Tensor<1, dim>> grad_jump(n_q_points);
      get_function_gradient_jump(fe_iv, solution, grad_jump);

      const double h = cell->face(f)->diameter();

      const double extent1 = cell->measure() / cell->face(f)->measure();
      const double extent2 = ncell->measure() / ncell->face(nf)->measure();
      const double penalty = get_penalty_factor(degree, extent1, extent2);

      double flux_jump_square = 0;
      double u_jump_square    = 0;
      for (unsigned int point = 0; point < n_q_points; ++point)
        {
          u_jump_square += jump[point] * jump[point] * JxW[point];
          const double flux_jump = grad_jump[point] * normals[point];
          flux_jump_square +=
            diffusion_coefficient * flux_jump * flux_jump * JxW[point];
        }
      copy_data_face.values[0] =
        0.5 * h * (flux_jump_square + penalty * u_jump_square);
      copy_data_face.values[1] = copy_data_face.values[0];
    };

    // Having computed local contributions for each cell, we still
    // need a way to copy these into the global vector that will hold
    // the error estimators for all cells:
    const auto copier = [&](const auto &copy_data) {
      if (copy_data.cell_index != numbers::invalid_unsigned_int)
        estimated_error_square_per_cell[copy_data.cell_index] +=
          copy_data.value;
      for (auto &cdf : copy_data.face_data)
        for (unsigned int j = 0; j < 2; ++j)
          estimated_error_square_per_cell[cdf.cell_indices[j]] += cdf.values[j];
    };

    // After all of this set-up, let's do the actual work: We resize
    // the vector into which the results will be written, and then
    // drive the whole process using the MeshWorker::mesh_loop()
    // function.
    estimated_error_square_per_cell.reinit(triangulation.n_active_cells());

    const UpdateFlags cell_flags =
      update_hessians | update_quadrature_points | update_JxW_values;
    const UpdateFlags face_flags = update_values | update_gradients |
                                   update_quadrature_points |
                                   update_JxW_values | update_normal_vectors;

    ScratchData scratch_data(
      mapping, fe, quadrature, cell_flags, face_quadrature, face_flags);

    CopyData copy_data;
    MeshWorker::mesh_loop(dof_handler.begin_active(),
                          dof_handler.end(),
                          cell_worker,
                          copier,
                          scratch_data,
                          copy_data,
                          MeshWorker::assemble_own_cells |
                            MeshWorker::assemble_own_interior_faces_once |
                            MeshWorker::assemble_boundary_faces,
                          boundary_worker,
                          face_worker);
  }

  // @sect3{The compute_energy_norm_error() function}
  // Next, we evaluate the accuracy in terms of the energy norm.
  // This function is similar to the assembling of the error estimator above.
  // Here we compute the square of the energy norm defined by
  // @f[
  //   \|u \|_{1,h}^2 = \sum_{K \in \Gamma_h} \nu\|\nabla u \|_K^2 +
  //   \sum_{f \in F_i} \sigma \| [ u ] \|_f^2 +
  //   \sum_{f \in F_b} \sigma  \|u\|_f^2.
  // @f]
  // Therefore the corresponding error is
  // @f[
  //   \|u -u_h \|_{1,h}^2 = \sum_{K \in \Gamma_h} \nu\|\nabla (u_h - u)  \|_K^2
  //   + \sum_{f \in F_i} \sigma  \|[ u_h ] \|_f^2 + \sum_{f \in F_b}\sigma
  //   \|u_h-g_D\|_f^2.
  // @f]
  template <int dim>
  double SIPGLaplace<dim>::compute_energy_norm_error()
  {
    energy_norm_square_per_cell.reinit(triangulation.n_active_cells());

    // Assemble $\sum_{K \in \Gamma_h} \nu\|\nabla (u_h - u)  \|_K^2 $.
    const auto cell_worker =
      [&](const auto &cell, auto &scratch_data, auto &copy_data) {
        const FEValues<dim> &fe_v = scratch_data.reinit(cell);

        copy_data.cell_index = cell->active_cell_index();

        const auto &               q_points   = fe_v.get_quadrature_points();
        const unsigned int         n_q_points = q_points.size();
        const std::vector<double> &JxW        = fe_v.get_JxW_values();

        std::vector<Tensor<1, dim>> grad_u(n_q_points);
        fe_v.get_function_gradients(solution, grad_u);

        std::vector<Tensor<1, dim>> grad_exact(n_q_points);
        exact_solution->gradient_list(q_points, grad_exact);

        double norm_square = 0;
        for (unsigned int point = 0; point < n_q_points; ++point)
          {
            norm_square +=
              (grad_u[point] - grad_exact[point]).norm_square() * JxW[point];
          }
        copy_data.value = diffusion_coefficient * norm_square;
      };

    // Assemble $\sum_{f \in F_b}\sigma  \|u_h-g_D\|_f^2$.
    const auto boundary_worker = [&](const auto &        cell,
                                     const unsigned int &face_no,
                                     auto &              scratch_data,
                                     auto &              copy_data) {
      const FEFaceValuesBase<dim> &fe_fv = scratch_data.reinit(cell, face_no);

      const auto &   q_points   = fe_fv.get_quadrature_points();
      const unsigned n_q_points = q_points.size();

      const std::vector<double> &JxW = fe_fv.get_JxW_values();

      std::vector<double> g(n_q_points);
      exact_solution->value_list(q_points, g);

      std::vector<double> sol_u(n_q_points);
      fe_fv.get_function_values(solution, sol_u);

      const double extent1 = cell->measure() / cell->face(face_no)->measure();
      const double penalty = get_penalty_factor(degree, extent1, extent1);

      double difference_norm_square = 0.;
      for (unsigned int point = 0; point < q_points.size(); ++point)
        {
          const double diff = (g[point] - sol_u[point]);
          difference_norm_square += diff * diff * JxW[point];
        }
      copy_data.value += penalty * difference_norm_square;
    };

    // Assemble $\sum_{f \in F_i} \sigma  \| [ u_h ] \|_f^2$.
    const auto face_worker = [&](const auto &        cell,
                                 const unsigned int &f,
                                 const unsigned int &sf,
                                 const auto &        ncell,
                                 const unsigned int &nf,
                                 const unsigned int &nsf,
                                 auto &              scratch_data,
                                 auto &              copy_data) {
      const FEInterfaceValues<dim> &fe_iv =
        scratch_data.reinit(cell, f, sf, ncell, nf, nsf);

      copy_data.face_data.emplace_back();
      CopyDataFace &copy_data_face = copy_data.face_data.back();

      copy_data_face.cell_indices[0] = cell->active_cell_index();
      copy_data_face.cell_indices[1] = ncell->active_cell_index();

      const std::vector<double> &JxW = fe_iv.get_JxW_values();

      const auto &       q_points   = fe_iv.get_quadrature_points();
      const unsigned int n_q_points = q_points.size();

      std::vector<double> jump(n_q_points);
      get_function_jump(fe_iv, solution, jump);

      const double extent1 = cell->measure() / cell->face(f)->measure();
      const double extent2 = ncell->measure() / ncell->face(nf)->measure();
      const double penalty = get_penalty_factor(degree, extent1, extent2);

      double u_jump_square = 0;
      for (unsigned int point = 0; point < n_q_points; ++point)
        {
          u_jump_square += jump[point] * jump[point] * JxW[point];
        }
      copy_data_face.values[0] = 0.5 * penalty * u_jump_square;
      copy_data_face.values[1] = copy_data_face.values[0];
    };

    const auto copier = [&](const auto &copy_data) {
      if (copy_data.cell_index != numbers::invalid_unsigned_int)
        energy_norm_square_per_cell[copy_data.cell_index] += copy_data.value;
      for (auto &cdf : copy_data.face_data)
        for (unsigned int j = 0; j < 2; ++j)
          energy_norm_square_per_cell[cdf.cell_indices[j]] += cdf.values[j];
    };

    const UpdateFlags cell_flags =
      update_gradients | update_quadrature_points | update_JxW_values;
    UpdateFlags face_flags =
      update_values | update_quadrature_points | update_JxW_values;

    const ScratchData scratch_data(mapping,
                                   fe,
                                   quadrature_overintegration,
                                   cell_flags,
                                   face_quadrature_overintegration,
                                   face_flags);

    CopyData copy_data;
    MeshWorker::mesh_loop(dof_handler.begin_active(),
                          dof_handler.end(),
                          cell_worker,
                          copier,
                          scratch_data,
                          copy_data,
                          MeshWorker::assemble_own_cells |
                            MeshWorker::assemble_own_interior_faces_once |
                            MeshWorker::assemble_boundary_faces,
                          boundary_worker,
                          face_worker);
    const double energy_error =
      std::sqrt(energy_norm_square_per_cell.l1_norm());
    return energy_error;
  }

  // @sect3{The refine_grid() function}
  template <int dim>
  void SIPGLaplace<dim>::refine_grid()
  {
    const double refinement_fraction = 0.1;

    GridRefinement::refine_and_coarsen_fixed_number(
      triangulation, estimated_error_square_per_cell, refinement_fraction, 0.);

    triangulation.execute_coarsening_and_refinement();
  }

  // @sect3{The compute_errors() function}
  // We compute three errors in the $L_2$ norm, $H_1$ seminorm, and
  // the energy norm, respectively. These are then printed to screen,
  // but also stored in a table that records how these errors decay
  // with mesh refinement and which can be output in one step at the
  // end of the program.
  template <int dim>
  void SIPGLaplace<dim>::compute_errors()
  {
    double L2_error, H1_error, energy_error;

    {
      Vector<float> difference_per_cell(triangulation.n_active_cells());
      VectorTools::integrate_difference(mapping,
                                        dof_handler,
                                        solution,
                                        *(exact_solution.get()),
                                        difference_per_cell,
                                        quadrature_overintegration,
                                        VectorTools::L2_norm);

      L2_error = VectorTools::compute_global_error(triangulation,
                                                   difference_per_cell,
                                                   VectorTools::L2_norm);
      convergence_table.add_value("L2", L2_error);
    }

    {
      Vector<float> difference_per_cell(triangulation.n_active_cells());
      VectorTools::integrate_difference(mapping,
                                        dof_handler,
                                        solution,
                                        *(exact_solution.get()),
                                        difference_per_cell,
                                        quadrature_overintegration,
                                        VectorTools::H1_seminorm);

      H1_error = VectorTools::compute_global_error(triangulation,
                                                   difference_per_cell,
                                                   VectorTools::H1_seminorm);
      convergence_table.add_value("H1", H1_error);
    }

    {
      energy_error = compute_energy_norm_error();
      convergence_table.add_value("Energy", energy_error);
    }

    std::cout << "  Error in the L2 norm         : " << L2_error << std::endl
              << "  Error in the H1 seminorm     : " << H1_error << std::endl
              << "  Error in the energy norm     : " << energy_error
              << std::endl;
  }

  // @sect3{The run() function}
  template <int dim>
  void SIPGLaplace<dim>::run()
  {
    const unsigned int max_cycle =
      (test_case == TestCase::convergence_rate ? 6 : 20);
    for (unsigned int cycle = 0; cycle < max_cycle; ++cycle)
      {
        std::cout << "Cycle " << cycle << std::endl;

        switch (test_case)
          {
            case TestCase::convergence_rate:
              {
                if (cycle == 0)
                  {
                    GridGenerator::hyper_cube(triangulation);

                    triangulation.refine_global(2);
                  }
                else
                  {
                    triangulation.refine_global(1);
                  }
                break;
              }

            case TestCase::l_singularity:
              {
                if (cycle == 0)
                  {
                    GridGenerator::hyper_L(triangulation);
                    triangulation.refine_global(3);
                  }
                else
                  {
                    refine_grid();
                  }
                break;
              }

            default:
              {
                Assert(false, ExcNotImplemented());
              }
          }

        std::cout << "  Number of active cells       : "
                  << triangulation.n_active_cells() << std::endl;
        setup_system();

        std::cout << "  Number of degrees of freedom : " << dof_handler.n_dofs()
                  << std::endl;

        assemble_system();
        solve();
        output_results(cycle);
        {
          convergence_table.add_value("cycle", cycle);
          convergence_table.add_value("cells", triangulation.n_active_cells());
          convergence_table.add_value("dofs", dof_handler.n_dofs());
        }
        compute_errors();

        if (test_case == TestCase::l_singularity)
          {
            compute_error_estimate();
            std::cout << "  Estimated error              : "
                      << std::sqrt(estimated_error_square_per_cell.l1_norm())
                      << std::endl;

            convergence_table.add_value(
              "Estimator",
              std::sqrt(estimated_error_square_per_cell.l1_norm()));
          }
        std::cout << std::endl;
      }

    // Having run all of our computations, let us tell the convergence
    // table how to format its data and output it to screen:
    convergence_table.set_precision("L2", 3);
    convergence_table.set_precision("H1", 3);
    convergence_table.set_precision("Energy", 3);

    convergence_table.set_scientific("L2", true);
    convergence_table.set_scientific("H1", true);
    convergence_table.set_scientific("Energy", true);

    if (test_case == TestCase::convergence_rate)
      {
        convergence_table.evaluate_convergence_rates(
          "L2", ConvergenceTable::reduction_rate_log2);
        convergence_table.evaluate_convergence_rates(
          "H1", ConvergenceTable::reduction_rate_log2);
      }
    if (test_case == TestCase::l_singularity)
      {
        convergence_table.set_precision("Estimator", 3);
        convergence_table.set_scientific("Estimator", true);
      }

    std::cout << "degree = " << degree << std::endl;
    convergence_table.write_text(
      std::cout, TableHandler::TextOutputFormat::org_mode_table);
  }
} // namespace Step74

// @sect3{The main() function}
// The following <code>main</code> function is similar to previous examples as
// well, and need not be commented on.
int main()
{
  try
    {
      using namespace dealii;
      using namespace Step74;

      const TestCase test_case = TestCase::l_singularity;

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

  return 0;
}


#include "interface.h"

#include <cmr/env.h>

#include "total_unimodularity.hpp"
#include "unimodularity.hpp"

#include <boost/numeric/ublas/io.hpp>

extern "C"
CMR_ERROR CMRinterfaceTU(CMR* cmr, CMR_CHRMAT* matrix, bool* pisTU, CMR_DEC** pdec, CMR_SUBMAT** psubmatrix)
{
  assert(cmr);
  assert(matrix);
  assert(pisTU);
  assert(!psubmatrix || !*psubmatrix);

  tu::integer_matrix mat(matrix->numRows, matrix->numColumns, 0);
  for (size_t row = 0; row < (size_t)matrix->numRows; ++row)
  {
    size_t first = matrix->rowSlice[row];
    size_t beyond = matrix->rowSlice[row + 1];
    for (size_t i = first; i < beyond; ++i)
    {
      size_t column = matrix->entryColumns[i];
      mat(row,column) = matrix->entryValues[i];
    }
  }

  tu::submatrix_indices violator;
  if (psubmatrix)
  {
    *pisTU = tu::is_totally_unimodular(mat, violator);
  }
  else
  {
    *pisTU = tu::is_totally_unimodular(mat);
  }

  if (*pisTU && pdec)
    fprintf(stderr, "Retrieval of decomposition is not implemented, yet.");

  if (!violator.rows.empty())
  {
    CMR_CALL( CMRsubmatCreate(cmr, violator.rows.size(), violator.columns.size(), psubmatrix) );
    CMR_SUBMAT* submatrix = *psubmatrix;
    for (size_t row = 0; row < submatrix->numRows; ++row)
      submatrix->rows[row] = violator.rows[row];
    for (size_t column = 0; column < submatrix->numColumns; ++column)
      submatrix->columns[column] = violator.columns[column];
  }

  return CMR_OKAY;
}

CMR_ERROR CMRinterfaceKModular(CMR* cmr, CMR_CHRMAT* matrix, size_t* pk)
{
  assert(cmr);
  assert(matrix);
  assert(pk);

  tu::integer_matrix mat(matrix->numRows, matrix->numColumns, 0);
  for (size_t row = 0; row < (size_t)matrix->numRows; ++row)
  {
    size_t first = matrix->rowSlice[row];
    size_t beyond = matrix->rowSlice[row + 1];
    for (size_t i = first; i < beyond; ++i)
    {
      size_t column = matrix->entryColumns[i];
      mat(row,column) = matrix->entryValues[i];
    }
  }

  size_t rank;
  unsigned int k;
  bool result = tu::is_k_modular(mat, rank, k);
  *pk = result ? k : 0;

  return CMR_OKAY;
}


#include <iostream>
#include <fstream>
#include <algorithm>
#include <array>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Shape_detection/Efficient_RANSAC.h>
#include <CGAL/structure_point_set.h>
#include <CGAL/Delaunay_triangulation_3.h>
#include <CGAL/Triangulation_vertex_base_with_info_3.h>
#include <CGAL/Advancing_front_surface_reconstruction.h>
#include <CGAL/IO/read_points.h>
#include <CGAL/disable_warnings.h>

#include <boost/lexical_cast.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel  Kernel;
typedef Kernel::Point_3  Point;
typedef std::pair<Kernel::Point_3, Kernel::Vector_3>         Point_with_normal;
typedef std::vector<Point_with_normal>                       Pwn_vector;
typedef CGAL::First_of_pair_property_map<Point_with_normal>  Point_map;
typedef CGAL::Second_of_pair_property_map<Point_with_normal> Normal_map;

// Efficient RANSAC types
typedef CGAL::Shape_detection::Efficient_RANSAC_traits
  <Kernel, Pwn_vector, Point_map, Normal_map>              Traits;
typedef CGAL::Shape_detection::Efficient_RANSAC<Traits>    Efficient_ransac;
typedef CGAL::Shape_detection::Plane<Traits>               Plane;

// Point set structuring type
typedef CGAL::Point_set_with_structure<Kernel>               Structure;

// Advancing front types
typedef CGAL::Advancing_front_surface_reconstruction_vertex_base_3<Kernel> LVb;
typedef CGAL::Advancing_front_surface_reconstruction_cell_base_3<Kernel> LCb;
typedef CGAL::Triangulation_data_structure_3<LVb,LCb> Tds;
typedef CGAL::Delaunay_triangulation_3<Kernel,Tds> Triangulation_3;
typedef Triangulation_3::Vertex_handle Vertex_handle;

typedef std::array<std::size_t,3> Facet;

// Functor to init the advancing front algorithm with indexed points
struct On_the_fly_pair{
  const Pwn_vector& points;
  typedef std::pair<Point, std::size_t> result_type;

  On_the_fly_pair(const Pwn_vector& points) : points(points) {}

  result_type
  operator()(std::size_t i) const
  {
    return result_type(points[i].first,i);
  }
};

// Specialized priority functor that favor structure coherence
template <typename Structure>
struct Priority_with_structure_coherence {

  Structure& structure;
  double bound;

  Priority_with_structure_coherence(Structure& structure,
                                    double bound)
    : structure (structure), bound (bound)
  {}

  template <typename AdvancingFront, typename Cell_handle>
  double operator() (AdvancingFront& adv, Cell_handle& c,
                     const int& index) const
  {
    // If perimeter > bound, return infinity so that facet is not used
    if (bound != 0)
      {
        double d  = 0;
        d = sqrt(squared_distance(c->vertex((index+1)%4)->point(),
                                  c->vertex((index+2)%4)->point()));
        if(d>bound) return adv.infinity();
        d += sqrt(squared_distance(c->vertex((index+2)%4)->point(),
                                   c->vertex((index+3)%4)->point()));
        if(d>bound) return adv.infinity();
        d += sqrt(squared_distance(c->vertex((index+1)%4)->point(),
                                   c->vertex((index+3)%4)->point()));
        if(d>bound) return adv.infinity();
      }

    Facet f = {{ c->vertex ((index + 1) % 4)->info (),
                 c->vertex ((index + 2) % 4)->info (),
                 c->vertex ((index + 3) % 4)->info () }};

    // facet_coherence takes values between -1 and 3, 3 being the most
    // coherent and -1 being incoherent. Smaller weight means higher
    // priority.
    double weight = 100. * (5 - structure.facet_coherence (f));

    return weight * adv.smallest_radius_delaunay_sphere (c, index);
  }

};

// Advancing front type
typedef CGAL::Advancing_front_surface_reconstruction
         <Triangulation_3,
          Priority_with_structure_coherence<Structure> >
        Reconstruction;

int main (int argc, char* argv[])
{
  // Points with normals.
  Pwn_vector points;

  const char* fname = (argc>1) ? argv[1] : "data/cube.pwn";
  // Loading point set from a file.

  if (!CGAL::read_points(fname, std::back_inserter(points),
                         CGAL::parameters::point_map(Point_map()).
                                           normal_map(Normal_map())))
  {
      std::cerr << "Error: cannot read file" << std::endl;
      return EXIT_FAILURE;
  }

  std::cerr << "Shape detection... ";

  Efficient_ransac ransac;
  ransac.set_input(points);
  ransac.add_shape_factory<Plane>(); // Only planes are useful for stucturing

  // Default RANSAC parameters
  Efficient_ransac::Parameters op;
  op.probability = 0.05;
  op.min_points = 100;
  op.epsilon = (argc>2 ? boost::lexical_cast<double>(argv[2]) : 0.002);
  op.cluster_epsilon = (argc>3 ? boost::lexical_cast<double>(argv[3]) : 0.02);
  op.normal_threshold = 0.7;

  ransac.detect(op); // Plane detection

  Efficient_ransac::Plane_range planes = ransac.planes();

  std::cerr << "done\nPoint set structuring... ";

  Pwn_vector structured_pts;
  Structure pss (points,
                 planes,
                 op.cluster_epsilon,  // Same parameter as RANSAC
                 CGAL::parameters::point_map (Point_map()).
                 normal_map (Normal_map()).
                 plane_map (CGAL::Shape_detection::Plane_map<Traits>()).
                 plane_index_map(CGAL::Shape_detection::Point_to_shape_index_map<Traits>(points, planes)));

  for (std::size_t i = 0; i < pss.size(); ++ i)
    structured_pts.push_back (pss[i]);

  std::cerr << "done\nAdvancing front... ";

  std::vector<std::size_t> point_indices(boost::counting_iterator<std::size_t>(0),
                                         boost::counting_iterator<std::size_t>(structured_pts.size()));

  Triangulation_3 dt (boost::make_transform_iterator(point_indices.begin(), On_the_fly_pair(structured_pts)),
                      boost::make_transform_iterator(point_indices.end(), On_the_fly_pair(structured_pts)));

  Priority_with_structure_coherence<Structure> priority (pss,
                                                         1000. * op.cluster_epsilon); // Avoid too large facets
  Reconstruction R(dt, priority);
  R.run ();

  std::cerr << "done\nWriting result... ";

  std::vector<Facet> output;
  const Reconstruction::TDS_2& tds = R.triangulation_data_structure_2();

  for(Reconstruction::TDS_2::Face_iterator fit = tds.faces_begin(); fit != tds.faces_end(); ++fit)
    if(fit->is_on_surface())
      output.push_back (CGAL::make_array(fit->vertex(0)->vertex_3()->id(),
                                         fit->vertex(1)->vertex_3()->id(),
                                         fit->vertex(2)->vertex_3()->id()));

  std::ofstream f ("out.off");
  f << "OFF\n" << structured_pts.size () << " " << output.size() << " 0\n"; // Header
  for (std::size_t i = 0; i < structured_pts.size (); ++ i)
    f << structured_pts[i].first << std::endl;
  for (std::size_t i = 0; i < output.size (); ++ i)
    f << "3 "
      << output[i][0] << " "
      << output[i][1] << " "
      << output[i][2] << std::endl;
  std::cerr << "all done\n" << std::endl;

  f.close();

  return 0;
}


//
// Created by Hamza El-Kebir on 6/18/21.
//

#ifndef LODESTAR_BACKWARDDIFFERENCE_HPP
#define LODESTAR_BACKWARDDIFFERENCE_HPP

#include <Eigen/Dense>

namespace ls {
    namespace primitives {
        template<typename TType, size_t TSamples, size_t TOrder, typename TScalarType = double>
        class BackwardDifference {
            static_assert(TSamples > 1, "Number of samples must be greater than one.");

            static_assert(TOrder > 1, "Differentiation order must be greater than one.");

            static_assert(TSamples > TOrder, "Number of samples must be greater than order.");

            static_assert(
                    // First order
                    (TSamples == 2 && TOrder == 1) ||
                    (TSamples == 3 && TOrder == 1) ||
                    (TSamples == 4 && TOrder == 1) ||
                    (TSamples == 5 && TOrder == 1) ||
                    (TSamples == 6 && TOrder == 1) ||
                    (TSamples == 7 && TOrder == 1) ||
                    // Second order
                    (TSamples == 3 && TOrder == 2) ||
                    (TSamples == 4 && TOrder == 2) ||
                    (TSamples == 5 && TOrder == 2) ||
                    (TSamples == 6 && TOrder == 2) ||
                    (TSamples == 7 && TOrder == 2) ||
                    // Third order
                    (TSamples == 4 && TOrder == 3) ||
                    (TSamples == 5 && TOrder == 3) ||
                    (TSamples == 6 && TOrder == 3) ||
                    (TSamples == 7 && TOrder == 3), "Combination of samples and order is not supported.");
        };

// ------- First derivative ---------

        /**
         * @brief Computes the first-order backward finite difference difference given two samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 2, 1, TScalarType> {
        protected:
            static TType compute(const TType &xNeg1, const TType &x0)
            {
                return x0 - xNeg1;
            }

        public:
            static TType compute(const TType &xNeg1, const TType &x0, TScalarType h)
            {
                return compute(xNeg1, x0) / h;
            }

            static const int kOrder = 1;
            static const int kErrorOrder = 1;
        };

        /**
         * @brief Computes the first-order backward finite difference difference given three samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 3, 1, TScalarType> {
        protected:
            static TType compute(const TType &xNeg2, const TType &xNeg1, const TType &x0)
            {
                return (3.0 / 2.0) * x0 - 2.0 * xNeg1 + (1.0 / 2.0) * xNeg2;
            }

        public:
            static TType compute(const TType &xNeg2, const TType &xNeg1, const TType &x0, TScalarType h)
            {
                return compute(xNeg2, xNeg1, x0) / h;
            }

            static const int kOrder = 1;
            static const int kErrorOrder = 2;
        };

        /**
         * @brief Computes the first-order backward finite difference difference given four samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 4, 1, TScalarType> {
        protected:
            static TType compute(const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0)
            {
                return (11.0 / 6.0) * x0 - 3.0 * xNeg1 + (3.0 / 2.0) * xNeg2 - (1.0 / 3.0) * xNeg3;
            }

        public:
            static TType
            compute(const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0, TScalarType h)
            {
                return compute(xNeg3, xNeg2, xNeg1, x0) / h;
            }

            static const int kOrder = 1;
            static const int kErrorOrder = 3;
        };

        /**
         * @brief Computes the first-order backward finite difference difference given five samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 5, 1, TScalarType> {
        protected:
            static TType
            compute(const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0)
            {
                return (25.0 / 12.0) * x0 - 4.0 * xNeg1 + 3.0 * xNeg2 - (4.0 / 3.0) * xNeg3 + (1.0 / 4.0) * xNeg4;
            }

        public:
            static TType
            compute(const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0,
                    TScalarType h)
            {
                return compute(xNeg4, xNeg3, xNeg2, xNeg1, x0) / h;
            }

            static const int kOrder = 1;
            static const int kErrorOrder = 4;
        };

        /**
         * @brief Computes the first-order backward finite difference difference given six samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 6, 1, TScalarType> {
        protected:
            static TType
            compute(const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1,
                    const TType &x0)
            {
                return (137.0 / 60.0) * x0 - 5.0 * xNeg1 + 5.0 * xNeg2 - (10.0 / 3.0) * xNeg3 + (5.0 / 4.0) * xNeg4 -
                       (1.0 / 6.0) * xNeg5;
            }

        public:
            static TType
            compute(const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1,
                    const TType &x0, TScalarType h)
            {
                return compute(xNeg5, xNeg4, xNeg3, xNeg2, xNeg1, x0) / h;
            }

            static const int kOrder = 1;
            static const int kErrorOrder = 5;
        };

        /**
         * @brief Computes the first-order backward finite difference difference given seven samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 7, 1, TScalarType> {
        protected:
            static TType
            compute(const TType &xNeg6, const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2,
                    const TType &xNeg1, const TType &x0)
            {
                return (49.0 / 20.0) * x0 - 6.0 * xNeg1 + (15.0 / 2.0) * xNeg2 - (20.0 / 3.0) * xNeg3 +
                       (15.0 / 4.0) * xNeg4 - (6.0 / 5.0) * xNeg5 + (1.0 / 6.0) * xNeg6;
            }

        public:
            static TType
            compute(const TType &xNeg6, const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2,
                    const TType &xNeg1, const TType &x0, TScalarType h)
            {
                return compute(xNeg6, xNeg5, xNeg4, xNeg3, xNeg2, xNeg1, x0) / h;
            }

            static const int kOrder = 1;
            static const int kErrorOrder = 6;
        };

// ------- Second derivative ---------

        /**
         * @brief Computes the second-order backward finite difference difference given three samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 3, 2, TScalarType> {
        protected:
            static TType compute(const TType &xNeg2, const TType &xNeg1, const TType &x0)
            {
                return x0 - 2 * xNeg1 + xNeg2;
            }

        public:
            static TType compute(const TType &xNeg2, const TType &xNeg1, const TType &x0, TScalarType h)
            {
                return compute(xNeg2, xNeg1, x0) / (h * h);
            }

            static const int kOrder = 2;
            static const int kErrorOrder = 1;
        };

        /**
         * @brief Computes the second-order backward finite difference difference given four samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 4, 2, TScalarType> {
        protected:
            static TType compute(const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0)
            {
                return 2 * x0 - 5 * xNeg1 + 4 * xNeg2 - 1 * xNeg3;
            }

        public:
            static TType
            compute(const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0, TScalarType h)
            {
                return compute(xNeg3, xNeg2, xNeg1, x0) / (h * h);
            }

            static const int kOrder = 2;
            static const int kErrorOrder = 2;
        };

        /**
         * @brief Computes the second-order backward finite difference difference given five samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 5, 2, TScalarType> {
        protected:
            static TType
            compute(const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0)
            {
                return (35.0 / 12.0) * x0 - (26.0 / 3.0) * xNeg1 + (19.0 / 2.0) * xNeg2 - (14.0 / 3.0) * xNeg3 +
                       (11.0 / 12.0) * xNeg4;
            }

        public:
            static TType
            compute(const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0,
                    TScalarType h)
            {
                return compute(xNeg4, xNeg3, xNeg2, xNeg1, x0) / (h * h);
            }

            static const int kOrder = 2;
            static const int kErrorOrder = 3;
        };

        /**
         * @brief Computes the second-order backward finite difference difference given six samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 6, 2, TScalarType> {
        protected:
            static TType
            compute(const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1,
                    const TType &x0)
            {
                return (15.0 / 4.0) * x0 - (77.0 / 6.0) * xNeg1 + (107.0 / 6.0) * xNeg2 - 13.0 * xNeg3 +
                       (61.0 / 12.0) * xNeg4 - (5.0 / 6.0) * xNeg5;
            }

        public:
            static TType
            compute(const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1,
                    const TType &x0, TScalarType h)
            {
                return compute(xNeg5, xNeg4, xNeg3, xNeg2, xNeg1, x0) / (h * h);
            }

            static const int kOrder = 2;
            static const int kErrorOrder = 4;
        };

        /**
         * @brief Computes the second-order backward finite difference difference given seven samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 7, 2, TScalarType> {
        protected:
            static TType
            compute(const TType &xNeg6, const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2,
                    const TType &xNeg1, const TType &x0)
            {
                return (203.0 / 45.0) * x0 - (87.0 / 5.0) * xNeg1 + (117.0 / 4.0) * xNeg2 - (254.0 / 9.0) * xNeg3 +
                       (33.0 / 2.0) * xNeg4 - (27.0 / 5.0) * xNeg5 + (137.0 / 180.0) * xNeg6;
            }

        public:
            static TType
            compute(const TType &xNeg6, const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2,
                    const TType &xNeg1, const TType &x0, TScalarType h)
            {
                return compute(xNeg6, xNeg5, xNeg4, xNeg3, xNeg2, xNeg1, x0) / (h * h);
            }

            static const int kOrder = 2;
            static const int kErrorOrder = 5;
        };

// ------- Third derivative ---------

        /**
         * @brief Computes the third-order backward finite difference difference given four samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 4, 3, TScalarType> {
        protected:
            static TType compute(const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0)
            {
                return 1.0 * x0 - 3.0 * xNeg1 + 3.0 * xNeg2 - 1.0 * xNeg3;
            }

        public:
            static TType
            compute(const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0, TScalarType h)
            {
                return compute(xNeg3, xNeg2, xNeg1, x0) / (h * h * h);
            }

            static const int kOrder = 3;
            static const int kErrorOrder = 1;
        };

        /**
         * @brief Computes the third-order backward finite difference difference given five samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 5, 3, TScalarType> {
        protected:
            static TType
            compute(const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0)
            {
                return (5.0 / 2.0) * x0 - 9.0 * xNeg1 + 12.0 * xNeg2 - 7.0 * xNeg3 + (3.0 / 2.0) * xNeg4;
            }

        public:
            static TType
            compute(const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0,
                    TScalarType h)
            {
                return compute(xNeg4, xNeg3, xNeg2, xNeg1, x0) / (h * h * h);
            }

            static const int kOrder = 3;
            static const int kErrorOrder = 2;
        };

        /**
         * @brief Computes the third-order backward finite difference difference given six samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 6, 3, TScalarType> {
        protected:
            static TType
            compute(const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1,
                    const TType &x0)
            {
                return (17.0 / 4.0) * x0 - (71.0 / 4.0) * xNeg1 + (59.0 / 2.0) * xNeg2 - (49.0 / 2.0) * xNeg3 +
                       (41.0 / 4.0) * xNeg4 - (7.0 / 4.0) * xNeg5;
            }

        public:
            static TType
            compute(const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1,
                    const TType &x0, TScalarType h)
            {
                return compute(xNeg5, xNeg4, xNeg3, xNeg2, xNeg1, x0) / (h * h * h);
            }

            static const int kOrder = 3;
            static const int kErrorOrder = 3;
        };

        /**
         * @brief Computes the third-order backward finite difference difference given seven samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 7, 3, TScalarType> {
        protected:
            static TType
            compute(const TType &xNeg6, const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2,
                    const TType &xNeg1, const TType &x0)
            {
                return (49.0 / 8.0) * x0 - 29.0 * xNeg1 + (461.0 / 8.0) * xNeg2 - 62.0 * xNeg3 + (307.0 / 8.0) * xNeg4 -
                       13.0 * xNeg5 + (15.0 / 8.0) * xNeg6;
            }

        public:
            static TType
            compute(const TType &xNeg6, const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2,
                    const TType &xNeg1, const TType &x0, TScalarType h)
            {
                return compute(xNeg6, xNeg5, xNeg4, xNeg3, xNeg2, xNeg1, x0) / (h * h * h);
            }

            static const int kOrder = 3;
            static const int kErrorOrder = 4;
        };

    }
}

#endif //LODESTAR_BACKWARDDIFFERENCE_HPP


#include <iostream>
#include "linop.h"
#include "utils.h"
#include "math.h"
#include <Eigen/Cholesky>

using namespace Rcpp;
using namespace RcppEigen;

RcppExport SEXP gls(SEXP X, SEXP S, SEXP Y, SEXP maxit, SEXP tol)
{
  using namespace Rcpp;
  using namespace RcppEigen;
  try {
    using Eigen::Map;
    using Eigen::MatrixXd;
    using Eigen::VectorXd;
    using Rcpp::List;

    typedef Map<VectorXd> MapVecd;
    typedef Map<Eigen::MatrixXd> MapMatd;

    const int maxiter(as<int>(maxit));
    const double toler(as<double>(tol));
    const Eigen::Map<MatrixXd> XX(as<MapMatd>(X));
    const Eigen::Map<MatrixXd> SS(as<MapMatd>(S));
    const Eigen::Map<VectorXd> YY(as<MapVecd>(Y));

    const int n(XX.rows());
    const int p(XX.cols());

    MatrixXd SX(MatrixXd(n, p));
    //MatrixXd XSX(MatrixXd(p, p));

    for (int i = 0; i < p; i++) {
      SX.col(i) = conjugate_gradient(SS, XX.col(i), maxiter, toler);
    }

    MatrixXd XSX((XX.adjoint()) * SX);
    VectorXd XSY((SX.adjoint()) * YY);

    VectorXd beta = conjugate_gradient(XSX, XSY, maxiter, toler);

    return List::create(Named("beta") = beta);
  } catch (std::exception &ex) {
    forward_exception_to_r(ex);
  } catch (...) {
    ::Rf_error("C++ exception (unknown reason)");
  }
  return R_NilValue; //-Wall
}

RcppExport SEXP gls_half_cg(SEXP X, SEXP S, SEXP Y, SEXP maxit, SEXP tol)
{
  using namespace Rcpp;
  using namespace RcppEigen;
  try {
    using Eigen::Map;
    using Eigen::MatrixXd;
    using Eigen::VectorXd;

    using Rcpp::List;

    typedef Map<VectorXd> MapVecd;
    typedef Map<Eigen::MatrixXd> MapMatd;

    const int maxiter(as<int>(maxit));
    const double toler(as<double>(tol));
    const Eigen::Map<MatrixXd> XX(as<MapMatd>(X));
    const Eigen::Map<MatrixXd> SS(as<MapMatd>(S));
    const Eigen::Map<VectorXd> YY(as<MapVecd>(Y));

    const int n(XX.rows());
    const int p(XX.cols());

    MatrixXd SX(MatrixXd(n, p));

    SX = SS.llt().solve(XX);

    MatrixXd XSX((XX.adjoint()) * SX);
    VectorXd XSY((SX.adjoint()) * YY);

    VectorXd beta = conjugate_gradient(XSX, XSY, maxiter, toler);

    return List::create(Named("beta") = beta);
  } catch (std::exception &ex) {
    forward_exception_to_r(ex);
  } catch (...) {
    ::Rf_error("C++ exception (unknown reason)");
  }
  return R_NilValue; //-Wall
}

RcppExport SEXP gls_direct(SEXP X, SEXP S, SEXP Y, SEXP maxit, SEXP tol)
{
  using namespace Rcpp;
  using namespace RcppEigen;
  try {
    using Eigen::Map;
    using Eigen::MatrixXd;
    using Eigen::VectorXd;

    using Rcpp::List;

    typedef Map<VectorXd> MapVecd;
    typedef Map<Eigen::MatrixXd> MapMatd;

    const int maxiter(as<int>(maxit));
    const double toler(as<double>(tol));
    const Eigen::Map<MatrixXd> XX(as<MapMatd>(X));
    const Eigen::Map<MatrixXd> SS(as<MapMatd>(S));
    const Eigen::Map<VectorXd> YY(as<MapVecd>(Y));

    const int n(XX.rows());
    const int p(XX.cols());

    MatrixXd SX(MatrixXd(n, p));

    SX = SS.llt().solve(XX);

    MatrixXd XSX((XX.adjoint()) * SX);
    VectorXd XSY((SX.adjoint()) * YY);

    VectorXd beta = XSX.ldlt().solve(XSY);

    return List::create(Named("beta") = beta);
  } catch (std::exception &ex) {
    forward_exception_to_r(ex);
  } catch (...) {
    ::Rf_error("C++ exception (unknown reason)");
  }
  return R_NilValue; //-Wall
}


/*
 * Filename: markers.cpp
 *
 * Copyright 2020 Tecnalia
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <manipulability_metrics/util/markers.h>

#include <eigen_conversions/eigen_kdl.h>
#include <eigen_conversions/eigen_msg.h>

#include <Eigen/Geometry>
#include <Eigen/SVD>

#include <algorithm>

namespace manipulability_metrics
{
visualization_msgs::Marker ellipsoidMarker(const Eigen::Matrix<double, 3, Eigen::Dynamic>& half_jacobian,
                                           const Eigen::Isometry3d& transform, const std::string& root,
                                           const std::string& ns, const std_msgs::ColorRGBA& color)
{
  auto svd = Eigen::JacobiSVD<Eigen::Matrix<double, 3, Eigen::Dynamic>>{ half_jacobian, Eigen::ComputeFullU };
  auto base = svd.matrixU();
  auto len = static_cast<Eigen::Vector3d>(svd.singularValues().topRows<3>());
  if (base.determinant() < 0)
  {
    base.col(1).swap(base.col(2));
    len.row(1).swap(len.row(2));
  }

  auto ellipsoid_marker = visualization_msgs::Marker{};
  ellipsoid_marker.type = visualization_msgs::Marker::SPHERE;
  ellipsoid_marker.header.frame_id = root;
  ellipsoid_marker.ns = ns;
  ellipsoid_marker.color = color;
  tf::pointEigenToMsg(transform.translation(), ellipsoid_marker.pose.position);
  tf::quaternionEigenToMsg(static_cast<Eigen::Quaterniond>(base), ellipsoid_marker.pose.orientation);
  tf::vectorEigenToMsg(len, ellipsoid_marker.scale);

  return ellipsoid_marker;
}

visualization_msgs::MarkerArray ellipsoidMarkers(const Chain& chain, const std::vector<double>& joint_positions,
                                                 const std::string& ns_prefix)
{
  return ellipsoidMarkers(chain, KDL::Frame{}, joint_positions, ns_prefix);
}

visualization_msgs::MarkerArray ellipsoidMarkers(const Chain& chain, const KDL::JntArray& joint_positions,
                                                 const std::string& ns_prefix)
{
  return ellipsoidMarkers(chain, KDL::Frame{}, joint_positions, ns_prefix);
}

visualization_msgs::MarkerArray ellipsoidMarkers(const Chain& chain, const KDL::Frame& tcp,
                                                 const std::vector<double>& joint_positions,
                                                 const std::string& ns_prefix)
{
  auto jnt_array = KDL::JntArray{ static_cast<unsigned int>(joint_positions.size()) };
  jnt_array.data =
      Eigen::Map<const Eigen::VectorXd>{ joint_positions.data(), static_cast<Eigen::Index>(joint_positions.size()) };
  return ellipsoidMarkers(chain, tcp, jnt_array, ns_prefix);
}

visualization_msgs::MarkerArray ellipsoidMarkers(const Chain& chain, const KDL::Frame& tcp,
                                                 const KDL::JntArray& joint_positions, const std::string& ns_prefix)
{
  auto frame = KDL::Frame{};
  auto jacobian = KDL::Jacobian{ static_cast<unsigned int>(chain.n_joints) };
  if (!chain.transform(joint_positions, tcp, frame) || !chain.jacobian(joint_positions, tcp.p, jacobian))
  {
    return {};
  }

  const auto transform = [&] {
    auto transform = Eigen::Isometry3d{};
    tf::transformKDLToEigen(frame, transform);
    return transform;
  }();

  auto marker_array = visualization_msgs::MarkerArray{};
  marker_array.markers.reserve(2);
  {
    std_msgs::ColorRGBA transparent_green;
    transparent_green.g = 1.0;
    transparent_green.a = 0.5;
    marker_array.markers.push_back(ellipsoidMarker(jacobian.data.topRows<3>(), transform, chain.root,
                                                   ns_prefix + "translational_ellipsoid", transparent_green));
  }
  {
    std_msgs::ColorRGBA transparent_red;
    transparent_red.r = 1.0;
    transparent_red.a = 0.5;
    marker_array.markers.push_back(ellipsoidMarker(jacobian.data.bottomRows<3>(), transform, chain.root,
                                                   ns_prefix + "rotational_ellipsoid", transparent_red));
  }

  return marker_array;
}

visualization_msgs::MarkerArray desiredEllipsoidMarkers(const Ellipsoid& desired_ellipsoid, const std::string& frame_id,
                                                        const KDL::Frame& transform)
{
  auto pseudoU = Eigen::Matrix<double, 6, 6>{};
  auto pseudoS = Eigen::Matrix<double, 6, 6>{ Eigen::Matrix<double, 6, 6>::Zero() };
  for (std::size_t i = 0; i < 6; ++i)
  {
    pseudoU.col(i) = desired_ellipsoid[i].unit;
    pseudoS(i, i) = desired_ellipsoid[i].len;
  }
  const auto pseudojac = pseudoU * pseudoS;

  const auto eigen_transform = [&] {
    auto eigen_transform = Eigen::Isometry3d{};
    tf::transformKDLToEigen(transform, eigen_transform);
    return eigen_transform;
  }();

  auto markers = visualization_msgs::MarkerArray{};
  markers.markers.reserve(2);
  {
    std_msgs::ColorRGBA transparent_light_green;
    transparent_light_green.r = 0.2;
    transparent_light_green.b = 0.2;
    transparent_light_green.g = 1.0;
    transparent_light_green.a = 0.5;

    markers.markers.push_back(ellipsoidMarker(pseudojac.topRows<3>(), eigen_transform, frame_id,
                                              "desired_translational_ellipsoid", transparent_light_green));
  }
  {
    std_msgs::ColorRGBA transparent_light_red;
    transparent_light_red.r = 1.0;
    transparent_light_red.b = 0.2;
    transparent_light_red.g = 0.2;
    transparent_light_red.a = 0.5;

    markers.markers.push_back(ellipsoidMarker(pseudojac.bottomRows<3>(), eigen_transform, frame_id,
                                              "desired_rotational_ellipsoid", transparent_light_red));
  }
  return markers;
}

}  // namespace manipulability_metrics


/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2017 Hidekazu Ikeno
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

///
/// \file fast_esprit.hpp
///
#ifndef MXPFIT_FAST_ESPRIT_HPP
#define MXPFIT_FAST_ESPRIT_HPP

#include <algorithm>
#include <type_traits>

#include <Eigen/Core>
#include <Eigen/Eigenvalues>

#include <mxpfit/exponential_sum.hpp>
#include <mxpfit/hankel_matrix.hpp>
#include <mxpfit/partial_lanczos_bidiagonalization.hpp>

// #include <mxpfit/matrix_free_gemv.hpp>
// #include <mxpfit/vandermonde_least_squares.hpp>
#include <mxpfit/prony_like_method_common.hpp>

namespace mxpfit
{
///
/// ### FastESPRIT
///
/// \brief Fast ESPRIT method for finding parameters of exponential sum
/// approximation from sampled data on uniform grid.
///
/// \tparam T  Scalar type of function values.
///
/// #### Description
///
/// For a given sequence \f$ f_{k}=f(t_{k}) \f$ sampled on a uniform grid
/// \f$t_{k}=t_{0}+hk\, (k=0,1,\dots,N-1)\f$ and prescribed accuracy
/// \f$\epsilon\f$, this class finds exponential sum approximation of function
/// \f$ f(t) \f$ such that,
///
/// \f[
///   \left| f_{k}-\sum_{j=1}^{M}c_{j}e^{-a_{j} t} \right| < \epsilon
/// \f]
///
/// with \f$\mathrm{Re}(a_{j}) > 0.\f$ The problem is solved using the fast
/// ESPRIT algorithm via partial Lanczos bidiagonalization which has been
/// developed by Potts and Tasche (2015). The present algorithm was slightly
/// modified from the original one.
///
/// #### References
///
/// 1. D. Potts and M. Tasche, "Fast ESPRIT algorithms based on partial singular
///    value decompositions", Appl. Numer. Math. **88** (2015) 31-45.
///    [DOI: https://doi.org/10.1016/j.apnum.2014.10.003]
/// 2. D. Potts and M. Tasche,"Parameter estimation for nonincreasing
///    exponential sums by Prony-like methods", Linear Algebra Appl. **439**
///    (2013) 1024-1039.
///    [DOI: https://doi.org/10.1016/j.laa.2012.10.036]
/// 3. K. Browne, S. Qiao, and Y. Wei, "A Lanczos bidiagonalization algorithm
///    for Hankel matrices", Linear Algebra Appl. **430** (2009) 1531-1543.
///    [DOI: https://doi.org/10.1016/j.laa.2008.01.012]
///
template <typename T>
class FastESPRIT
{
public:
    using Scalar        = T;
    using RealScalar    = typename Eigen::NumTraits<Scalar>::Real;
    using ComplexScalar = std::complex<RealScalar>;
    using Index         = Eigen::Index;

    using Vector        = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using RealVector    = Eigen::Matrix<RealScalar, Eigen::Dynamic, 1>;
    using ComplexVector = Eigen::Matrix<ComplexScalar, Eigen::Dynamic, 1>;

    using Matrix = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using RealMatrix =
        Eigen::Matrix<RealScalar, Eigen::Dynamic, Eigen::Dynamic>;
    using ComplexMatrix =
        Eigen::Matrix<ComplexScalar, Eigen::Dynamic, Eigen::Dynamic>;

    using ResultType =
        typename detail::gen_prony_like_method_result<T>::ResultType;

private:
    using HankelGEMV      = MatrixFreeGEMV<HankelMatrix<Scalar>>;
    using VandermondeGEMV = MatrixFreeGEMV<VandermondeMatrix<ComplexScalar>>;
    enum
    {
        IsComplex = Eigen::NumTraits<Scalar>::IsComplex,
        Alignment = Eigen::internal::traits<Matrix>::Alignment
    };

    using MappedMatrix = Eigen::Map<Matrix, Alignment>;

    Index m_rows;
    Index m_cols;
    Index m_max_terms;
    Index m_extra_basis = 5;
    HankelMatrix<Scalar> m_matH;
    PartialLanczosBidiagonalization<HankelGEMV> m_plbd;

public:
    ///
    /// Default constructor
    ///
    FastESPRIT() = default;

    ///
    /// Constructor with memory preallocation
    ///
    /// \param[in] N  Number of sampling points
    /// \param[in] L  Window size. This is equals to the number of rows of
    ///               generalized Hankel matrix.
    /// \param[in] M  Maxumum number of terms used for the exponential sum.
    /// \pre  `N >= M >= 1` and `N - L + 1 >= M >= 1`.
    ///
    FastESPRIT(Index N, Index L, Index M)
        : m_rows(L),
          m_cols(N - L + 1),
          m_max_terms(M),
          m_extra_basis(5),
          m_matH(m_rows, m_cols),
          m_plbd(m_rows, m_cols,
                 std::min({m_rows, m_cols, m_max_terms + m_extra_basis}))
    {
        assert(m_rows >= M && m_cols >= M && M >= 1);
    }

    /// Destructor
    ~FastESPRIT()
    {
    }

    ///
    /// Memory reallocation
    ///
    /// \param[in] N  Number of sampling points
    /// \param[in] L  Window size. This is equals to the number of rows of
    ///               generalized Hankel matrix.
    /// \param[in] M  Maxumum number of terms used for the exponential sum.
    /// \pre  `N >= L >= N / 2 >= 1` and `N - L + 1 >= M >= 1`.
    ///
    void resize(Index N, Index L, Index M)
    {
        m_rows = L;
        m_cols = N - L + 1;
        assert(m_rows >= M && m_cols >= M && M >= 1);
        m_max_terms = M;
        m_matH.resize(m_rows, m_cols);
        // Internal matrices of `m_plbd` are resized automatically during
        // computation
    }

    ///
    /// \return Number of sampling points.
    ///
    Index size() const
    {
        return m_matH.size();
    }

    ///
    /// Fit signals by a exponential sum
    ///
    /// \param[in] f The array of signals sampled on the equispaced grid. The
    ///    first `size()` elemnets of `f` are used as a sampled data. In case
    ///    `f.size() < size()` then, last `size() - f.size()` elements are
    ///    padded by zeros.
    /// \param[in] eps  Small positive number `(0 < eps < 1)` that
    ///    controlls the accuracy of the fit.
    /// \param[in] x0  Argument of first sampling point
    /// \param[in] delta Spacing between neighboring sample points.
    ///
    template <typename VectorT>
    ResultType compute(const Eigen::MatrixBase<VectorT>& h, RealScalar x0,
                       RealScalar delta, RealScalar eps);
};

template <typename T>
template <typename VectorT>
typename FastESPRIT<T>::ResultType
FastESPRIT<T>::compute(const Eigen::MatrixBase<VectorT>& h, RealScalar x0,
                       RealScalar delta, RealScalar eps)
{
    using Eigen::numext::sqrt;
    assert(h.size() == size() && "Number of data points mismatch.");
    //
    // Form rectangular Hankel matrix and pre-compute for fast multiplication to
    // the vector.
    //
    m_matH.setCoeffs(h);
    const Index nr = m_matH.rows();
    const Index nc = m_matH.cols();

    //-------------------------------------------------------------------------
    // Compute roots of Prony polynomials
    //-------------------------------------------------------------------------
    //
    // Partial Lanczos bidiagonalization of Hankel matrix H = P B Q^H
    //
    HankelGEMV opH(m_matH);
    m_plbd.setTolerance(eps);
    m_plbd.compute(opH, std::min({nr, nc, m_max_terms + m_extra_basis}));
    const Index nterms = m_plbd.rank();

    if (nterms == 0)
    {
        return ResultType();
    }

    // {
    //     Matrix denseH = m_matH.toDenseMatrix();
    //     std::cout << "Partial LBD error: ||H - P B Q*||_F  = "
    //               << (denseH - m_plbd.reconstructedMatrix()).norm()
    //               << std::endl;
    //     std::cout << "(alpha):\n"
    //               << m_plbd.diagonalAlpha().head(nterms) << "\n(beta):\n"
    //               << m_plbd.superdiagonalBeta().head(nterms - 1) <<
    //               std::endl;
    // }

    // --- Form the views of matrix Q
    // Matrix Q excluding the last row
    auto Q0 = m_plbd.matrixQ().block(0, 0, nc - 1, nterms);
    // Matrix Q excluding the first row
    auto Q1 = m_plbd.matrixQ().block(1, 0, nc - 1, nterms);
    // adjoint of the last row of matrix Q
    auto nu = m_plbd.matrixQ().block(nc - 1, 0, 1, nterms).adjoint();
    //
    // Compute the spectral matrix G = pinv(Q0) * Q1, where pinv indicate
    // the Moore-Penrose pseudo-inverse. The computation of the
    // pseudo-inverse of Q0 can be avoided.
    //
    Matrix G(Q0.adjoint() * Q1);
    Vector phi(G.adjoint() * nu);
    auto scal = RealScalar(1) / (RealScalar(1) - nu.squaredNorm());
    G += scal * nu * phi.adjoint();
    //
    // Prony roots \f$\{z_i\{\}\f$ are the eigenvalues of matrix G.
    // The exponents for approximation are obtained as \f$ \log z_i \f$
    //
    ComplexVector tmp_roots(G.eigenvalues());
    // std::cout << "\n*** roots\n" << tmp_roots << std::endl;
    //
    // Find roots on unit disk \f$z_{i} \in \mathbb{D}\f$.
    // If Scalar is a real type, neagative real roots will also be discarded.
    //
    tmp_roots =
        detail::prony_roots_on_unit_disk<Scalar>::compute(tmp_roots, sqrt(eps));

    ComplexVector roots(std::min(tmp_roots.size(), m_max_terms));
    roots = tmp_roots.head(roots.size());

    //----------------------------------------------------------------------
    // Solve overdetermined Vandermonde system to obtain the weights
    //----------------------------------------------------------------------
    ComplexVector weights(roots.size());
    detail::solve_overdetermined_vandermonde(
        roots, h, weights, eps, std::max(Index(100), 5 * roots.size()));

    // Rescale the exponents and weights, and return the result
    ResultType ret(roots.size());
    ret.exponents() = -roots.array().log() / delta;
    if (x0 == RealScalar())
    {
        ret.weights() = weights.array(); // Don't forget to copy weights
    }
    else
    {
        ret.weights() = weights.array() * (-x0 * ret.exponents()).exp();
    }

    return ret;

    // return detail::gen_prony_like_method_result<T>::create(
    //     roots.array(), weights.array(), x0, delta);
}

} // namespace mxpfit

#endif /* MXPFIT_FAST_ESPRIT_HPP */