/**
 * Matrix-free finite element method introduced in doi:10.1002/nme.5263
 *
 * This library provides classes to compute the modal stiffness
 * matrices and strain-displacement vectors in Fourier space for a
 * homogeneous, periodic unit-cell. Combined with a FFT library, these
 * can be used to compute the solution to any problem of homogeneous,
 * periodic linear elasticity.
 */

#pragma once

#include <array>
#include <cmath>
#include <concepts>
#include <numbers>
#include <numeric>

#include <complex>
#include <cstddef>
#include <iostream>
#include <stdexcept>
#include <string>

#include <Eigen/Dense>

namespace bri17 {
/**
 * A rectangular grid with fixed spacing in each direction.
 *
 * @tparam T the scalar type
 * @tparam DIM the number of spatial dimensions (must be 2 or 3)
 */
template <typename T, int DIM>
requires(std::floating_point<T> &&
         ((DIM == 2) || (DIM == 3))) class CartesianGrid {
 public:
  /** Number of nodes per cell: `2 ** DIM`. */
  static constexpr int num_nodes_per_cell = 1 << DIM;

  /** Number of cells in each direction. */
  std::array<int, DIM> const shape;

  /** Size of the grid in each direction (arbitrary units of length). */
  std::array<T, DIM> const L;

  /** Total number of cells: `shape[0] * shape[1] * ... * shape[DIM-1]`. */
  int const size;

  /**
   * @param shape number of cells in each direction
   * @param L size of the grid in each direction (arbitrary units of length)
   */
  CartesianGrid(std::array<int, DIM> shape, std::array<T, DIM> L)
      : shape{shape},
        L{L},
        size{std::reduce(shape.cbegin(), shape.cend(), int{1},
                         std::multiplies())} {}

  /** Return a string representation of this object. */
  std::string repr() const {
    std::ostringstream stream;
    stream << "CartesianGrid<" << typeid(T).name() << "," << DIM << ">{shape={";
    for (auto n : shape) stream << n << ",";
    stream << "},L={";
    for (auto x : L) stream << x << ",";
    stream << "}}";
    return stream.str();
  }

  /**
   * Return the index of the node located at <tt>[i, j]</tt>.
   *
   * This method cannot be called with a 3D grid (this condition is
   * checked at compile time). Nodes numbering follows the row-major
   * order convention.
   */
  int get_node_at(int i, int j) const {
    static_assert(DIM == 2, "this method expects a 2D grid");
    return i * shape[1] + j;
  }

  /**
   * Return the index of the node located at <tt>[i, j, k]</tt>.
   *
   * This method cannot be called with a 2D grid (this condition is
   * checked at compile time). Nodes numbering follows the row-major
   * order convention.
   */
  int get_node_at(int i, int j, int k) const {
    static_assert(DIM == 3, "this method expects a 3D grid");
    return (i * shape[1] + j) * shape[2] + k;
  }

  /**
   * Return the indices of the vertices of a specific cell.
   *
   * Numbering of vertices in 2D
   *
   * ```
   * 2────4
   * │    │
   * │    │
   * 1────3
   * ```
   *
   *
   * Numbering of vertices in 3D
   *
   * ```
   *      4────────8
   *     ╱│       ╱│
        ╱ │      ╱ │
   *   ╱  │     ╱  │
   *  ╱   │    ╱   │
   * 2────────6 ───7
   * │   ╱3   │   ╱
   * │  ╱     │  ╱
     │ ╱      │ ╱
   * │╱       │╱
   * 1────────5
   * ```
   *
   * @param cell index of the cell (row-major order)
   * @return array of node indices
   */
  std::array<int, num_nodes_per_cell> get_cell_nodes(int cell) const {
    std::array<int, num_nodes_per_cell> nodes;
    if constexpr (DIM == 2) {
      const int i1 = cell / shape[1];
      const int j1 = cell % shape[1];
      const int i2 = i1 == shape[0] - 1 ? 0 : i1 + 1;
      const int j2 = j1 == shape[1] - 1 ? 0 : j1 + 1;
      nodes[0] = get_node_at(i1, j1);
      nodes[1] = get_node_at(i1, j2);
      nodes[2] = get_node_at(i2, j1);
      nodes[3] = get_node_at(i2, j2);
    } else if constexpr (DIM == 3) {
      const int k1 = cell % shape[2];
      const int ij1 = cell / shape[2];
      const int j1 = ij1 % shape[1];
      const int i1 = ij1 / shape[1];
      const int i2 = i1 == shape[0] - 1 ? 0 : i1 + 1;
      const int j2 = j1 == shape[1] - 1 ? 0 : j1 + 1;
      const int k2 = k1 == shape[2] - 1 ? 0 : k1 + 1;
      nodes[0] = get_node_at(i1, j1, k1);
      nodes[1] = get_node_at(i1, j1, k2);
      nodes[2] = get_node_at(i1, j2, k1);
      nodes[3] = get_node_at(i1, j2, k2);
      nodes[4] = get_node_at(i2, j1, k1);
      nodes[5] = get_node_at(i2, j1, k2);
      nodes[6] = get_node_at(i2, j2, k1);
      nodes[7] = get_node_at(i2, j2, k2);
    } else {
      throw std::logic_error("This should never occur");
    }
    return nodes;
  }
};

/** Print the grid to the specified `ostream`. */
template <typename T, int DIM>
std::ostream &operator<<(std::ostream &os, const CartesianGrid<T, DIM> &grid) {
  return os << grid.repr();
}

/**
 * Implementation of the results of [Bri17] per se.
 *
 * This class provides methods to compute the modal strain-displacement and
 * stiffness matrices.
 *
 * @tparam T the scalar type
 * @tparam DIM the number of spatial dimensions (must be 2 or 3)
 */
template <typename T, int DIM>
requires(std::floating_point<T> && ((DIM == 2) || (DIM == 3))) class Hooke {
 public:
  /** The shear modulus of the material. */
  T const mu;

  /** The Poisson ratio of the material. */
  T const nu;

  /** Geometric description of the underlying FE grid. */
  CartesianGrid<T, DIM> const grid;

  /**
   * @param mu shear modulus
   * @param nu Poisson ratio
   * @param grid the FE grid
   */
  Hooke(T mu, T nu, CartesianGrid<T, DIM> &grid)
      : mu{mu}, nu{nu}, grid{grid} {};

  /** Return a string representation of this object. */
  std::string repr() const {
    std::ostringstream stream;
    stream << "Hooke<" << typeid(T).name() << "," << DIM << ">{mu=" << mu
           << ",nu=" << nu << ",grid=" << grid << std::endl;
    return stream.str();
  }

  /**
   * Compute modal strain-displacement vector for specified spatial frequency.
   *
   * The output parameter `B` must be a preallocated array of size `DIM`.
   *
   * @param k the multi-index in the frequency domain
   * @param B the strain-displacement vector `B^[k, :]` (output parameter)
   */
  void modal_strain_displacement(int const *k, std::complex<T> *B) const {
    T c[DIM];
    T s[DIM];
    T sum_alpha{};  // TODO Check that initializes to 0

    for (int i = 0; i < DIM; i++) {
      T alpha = std::numbers::pi_v<T> * k[i] / grid.shape[i];
      sum_alpha += alpha;
      c[i] = cos(alpha);
      s[i] = sin(alpha) * grid.shape[i] / grid.L[i];
    }

    std::complex<T> prefactor{-2 * sin(sum_alpha), 2 * cos(sum_alpha)};

    if constexpr (DIM == 2) {
      B[0] = prefactor * s[0] * c[1];
      B[1] = prefactor * c[0] * s[1];
    } else if constexpr (DIM == 3) {
      B[0] = prefactor * s[0] * c[1] * c[2];
      B[1] = prefactor * c[0] * s[1] * c[2];
      B[2] = prefactor * c[0] * c[1] * s[2];
    } else {
      throw std::logic_error("this should never occur");
    }
  }

  /**
   * Compute modal stiffness matrix for specified spatial frequency.
   *
   * The output parameter `K` must be a preallocated array of size
   * `DIM * DIM`.
   *
   * @param k the multi-index in the frequency domain
   * @param K the stiffness matrix `K^[k, :, :]` (output parameter)
   */
  void modal_stiffness(int const *k, std::complex<T> *K) const {
    // In the notation of [Bri17, see Eq. (B.17)]
    //
    // phi[i] = phi(z_i) / h_i
    // chi[i] = chi(z_i) * h_i
    // psi[i] = psi(z_i)
    //
    // Which simplifies the expression of H_k (there are no h_i's).
    T phi[DIM];
    T psi[DIM];
    T chi[DIM];
    for (int i = 0; i < DIM; i++) {
      T h = grid.L[i] / grid.shape[i];
      T beta = 2 * std::numbers::pi_v<T> * k[i] / grid.shape[i];
      phi[i] = 2 * (1 - cos(beta)) / h / h;
      chi[i] = (2 + cos(beta)) / 3;
      psi[i] = sin(beta) / h;
    }

    const double scaling = mu / (1. - 2. * nu);
    if constexpr (DIM == 2) {
      auto H_00 = phi[0] * chi[1];
      auto H_11 = chi[0] * phi[1];
      auto K_diag = mu * (H_00 + H_11);
      K[0] = scaling * H_00 + K_diag;
      K[1] = scaling * psi[0] * psi[1];
      K[2] = K[1];
      K[3] = scaling * H_11 + K_diag;
    } else if constexpr (DIM == 3) {
      auto H_00 = phi[0] * chi[1] * chi[2];
      auto H_11 = chi[0] * phi[1] * chi[2];
      auto H_22 = chi[0] * chi[1] * phi[2];
      auto K_diag = mu * (H_00 + H_11 + H_22);
      K[0] = scaling * H_00 + K_diag;             // [0, 0]
      K[1] = scaling * psi[0] * psi[1] * chi[2];  // [0, 1]
      K[2] = scaling * psi[0] * chi[1] * psi[2];  // [0, 2]
      K[3] = K[1];                                // [1, 0]
      K[4] = scaling * H_11 + K_diag;             // [1, 1]
      K[5] = scaling * chi[0] * psi[1] * psi[2];  // [1, 2]
      K[6] = K[2];                                // [2, 0]
      K[7] = K[5];                                // [2, 1]
      K[8] = scaling * H_22 + K_diag;             // [2, 2]
    } else {
      throw std::logic_error("this should never occur");
    }
  }

  /**
   * Compute the strains induced by the specified eigenstresses.
   *
   * The eigenstresses `τ[n, i, j]` are constant in each cell n. They induce the
   * average strains `ε[n, i, j]`.
   *
   * This method computes the **opposite** of the induced strain!
   *
   * @param k multi-index of the Fourier component
   * @param tau the `k`-th Fourier component of the eigenstress `τ`,
   *            `τ^[k, :, :]`
   * @param eta the `k`-th Fourier component of `-ε`, `-ε^[k, :, :]`
   *            (output parameter).
   */
  void modal_eigenstress_to_opposite_strain(int const *k,
                                            std::complex<T> const *tau,
                                            std::complex<T> *eta) const {
    using Vector = Eigen::Matrix<std::complex<T>, DIM, 1>;
    using Matrix = Eigen::Matrix<std::complex<T>, DIM, DIM>;
    constexpr T const sqrt2 = std::numbers::sqrt2_v<T>;
    constexpr std::complex<T> zero{};
    constexpr int const sym = DIM == 2 ? 3 : 6;
    Vector B{};
    modal_strain_displacement(k, B.data());
    Matrix K{};
    modal_stiffness(k, K.data());
    Matrix tau_mat;
    bool null_frequency = false;
    if constexpr (DIM == 2) {
      // clang-format off
      tau_mat <<         tau[0], tau[2] / sqrt2,
                 tau[2] / sqrt2,         tau[1];
      // clang-format on
      null_frequency = (k[0] == 0) && (k[1] == 0);
    } else if constexpr (DIM == 3) {
      // clang-format off
      tau_mat <<         tau[0], tau[5] / sqrt2, tau[4] / sqrt2,
                 tau[5] / sqrt2,         tau[1], tau[3] / sqrt2,
                 tau[4] / sqrt2, tau[3] / sqrt2,         tau[2];
      // clang-format on
      null_frequency = (k[0] == 0) && (k[1] == 0) && (k[2] == 0);
    }
    if (null_frequency) {
      for (int i = 0; i < sym; i++) eta[i] = zero;
      return;
    }
    Vector rhs = tau_mat * B.conjugate();
    Vector u = K.llt().solve(rhs);
    Matrix eta_mat = 0.5 * (B * u.transpose() + u * B.transpose());
    if constexpr (DIM == 2) {
      eta[0] = eta_mat(0, 0);
      eta[1] = eta_mat(1, 1);
      eta[2] = sqrt2 * eta_mat(0, 1);
    } else if constexpr (DIM == 3) {
      eta[0] = eta_mat(0, 0);
      eta[1] = eta_mat(1, 1);
      eta[2] = eta_mat(2, 2);
      eta[3] = sqrt2 * eta_mat(1, 2);
      eta[4] = sqrt2 * eta_mat(2, 0);
      eta[5] = sqrt2 * eta_mat(0, 1);
    }
  }
};

/** Print the grid to the specified `ostream`. */
template <typename T, int DIM>
std::ostream &operator<<(std::ostream &os, const Hooke<T, DIM> &hooke) {
  return os << hooke.repr();
}

}  // namespace bri17


#ifndef MATH_HPP
#define MATH_HPP

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

template<class T, int M = Eigen::Dynamic, int N = Eigen::Dynamic>
using matrix = Eigen::Matrix<T, M, N>;

template<class T, int M = Eigen::Dynamic>
using vector = matrix<T, M, 1>;

using real = double;

using vec = vector<real>;

using vec4 = vector<real, 4>;
using vec3 = vector<real, 3>;
using vec2 = vector<real, 2>;
using vec1 = vector<real, 1>;

using mat = matrix<real>;
using mat4x4 = matrix<real, 4, 4>;
using mat3x3 = matrix<real, 3, 3>;

template<class T>
using quaternion = Eigen::Quaternion<T>;

using quat = quaternion<real>;

struct rigid {
  quat orient;
  vec3 pos;

  rigid(): orient(1, 0, 0, 0), pos(0, 0, 0) {}

  static rigid translation(real x, real y, real z) {
    rigid res;
    res.pos = {x, y, z};
    return res;
  }

  static rigid translation(vec3 t) {
    return translation(t.x(), t.y(), t.z());
  }

  static rigid rotation(quat q) {
    rigid res;
    res.orient = q;
    return res;
  }

  rigid operator*(const rigid& other) const {
    rigid res;
    res.orient = orient * other.orient;
    res.pos = pos + orient * other.pos;
    return res;
  }

  rigid inv() const {
    rigid res;
    res.orient = orient.conjugate();
    res.pos = -(res.orient * pos);
    return res;
  }

};

#endif


#include <Eigen/Dense>
#include <unsupported/Eigen/AutoDiff>
#include "gtest/gtest.h"
#define SOLVER_DEBUG
#define SQP_SOLVER_PRINTING
#define QP_SOLVER_PRINTING
#define SOLVER_ASSERT(x) EXPECT_TRUE(x)
#include "solvers/sqp.hpp"

using namespace sqp;

namespace sqp_test_autodiff {

template <typename _Derived, typename _Scalar, int _VAR_SIZE, int _NUM_EQ=0, int _NUM_INEQ=0>
struct ProblemBase {
    enum {
        VAR_SIZE = _VAR_SIZE,
        NUM_EQ = _NUM_EQ,
        NUM_INEQ = _NUM_INEQ,
    };

    using Scalar = double;
    using var_t = Eigen::Matrix<Scalar, VAR_SIZE, 1>;
    using grad_t = Eigen::Matrix<Scalar, VAR_SIZE, 1>;
    using hessian_t = Eigen::Matrix<Scalar, VAR_SIZE, VAR_SIZE>;
    using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;

    using b_eq_t = Eigen::Matrix<Scalar, NUM_EQ, 1>;
    using A_eq_t = Eigen::Matrix<Scalar, NUM_EQ, VAR_SIZE>;
    using b_ineq_t = Eigen::Matrix<Scalar, NUM_INEQ, 1>;
    using A_ineq_t = Eigen::Matrix<Scalar, NUM_INEQ, VAR_SIZE>;
    using box_t = var_t;

    using ADScalar = Eigen::AutoDiffScalar<grad_t>;
    using ad_var_t = Eigen::Matrix<ADScalar, VAR_SIZE, 1>;
    using ad_eq_t = Eigen::Matrix<ADScalar, NUM_EQ, 1>;
    using ad_ineq_t = Eigen::Matrix<ADScalar, NUM_INEQ, 1>;

    template <typename vec>
    void AD_seed(vec &x)
    {
        for (int i=0; i<x.rows(); i++) {
            x[i].derivatives().coeffRef(i) = 1;
        }
    }

    void cost_linearized(const var_t& x, grad_t &grad, Scalar &cst)
    {
        ad_var_t _x = x;
        ADScalar _cst;
        AD_seed(_x);
        /* Static polymorphism using CRTP */
        static_cast<_Derived*>(this)->cost(_x, _cst);
        cst = _cst.value();
        grad = _cst.derivatives();
    }

    void constraint_linearized(const var_t& x, A_eq_t& A_eq, b_eq_t& b_eq, A_ineq_t& A_ineq, b_ineq_t& b_ineq, box_t& lbx, box_t& ubx)
    {
        ad_eq_t ad_eq;
        ad_ineq_t ad_ineq;

        ad_var_t _x = x;
        AD_seed(_x);
        static_cast<_Derived*>(this)->constraint(_x, ad_eq, ad_ineq, lbx, ubx);

        for (int i = 0; i < ad_eq.rows(); i++) {
            b_eq[i] = ad_eq[i].value();
            Eigen::Ref<MatX> deriv = ad_eq[i].derivatives().transpose();
            A_eq.row(i) = deriv;
        }

        for (int i = 0; i < ad_ineq.rows(); i++) {
            b_ineq[i] = ad_ineq[i].value();
            Eigen::Ref<MatX> deriv = ad_ineq[i].derivatives().transpose();
            A_ineq.row(i) = deriv;
        }
    }
};

struct SOFCModel : public ProblemBase<SOFCModel,
                                double,
                                /* Nx    */5,
                                /* Neq   */1,
                                /* Nineq */2>  {
    const Scalar delta_air = 1e-5;
    const Scalar delta_cool = 1e-4;
    const Scalar LHV_CH4 = 833.33;      // [J/kg]
    const Scalar N_cell = 70;
    const Scalar F = 96486.00/16e3;    // [C/kg]
    const Scalar Pel_ref = 1000.00;    // [W]
    //const Scalar conv_Lm_to_mols = 7.4356e-4;    // [W]

    //Eigen::Matrix<Scalar, 2, 1> SOLUTION = {0.7071067812, 0.707106781};

    template <typename DerivedA, typename DerivedB>
    void cost(const DerivedA& x, DerivedB &cst)
    {
        // x =[u (4), U_cell(1), theta(15)]
        // cst = -N_cell * x(4) * x(2)/(x(0) * LHV_CH4) + delta_air * pow(x(1), 2) + delta_cool * pow(x(3), 2);
        cst = -1*N_cell * x(4) * x(2) + 1*(1*delta_air * pow(x(1), 2) + 1*delta_cool * pow(x(3), 2))*(x(0) * LHV_CH4);
    }

    template <typename A, typename B, typename C>
    void constraint(const A& x, B& eq, C& ineq, box_t& lbx, box_t& ubx)
    {
        // nu < 0.8, lambda > 4
        ineq << N_cell*x(2) - 0.8*(8*F*x(0)), 4.0*(2.0*x(0)) - x(1);
        //ineq << N_cell*x(2)/(8*F*x(0)) - 0.8, 3.0 - x(1)/(2.0*x(0));
        // x^2 + y^2 == 1
        // eq << x.squaredNorm() - 1;
        // SOFC dynamics, function - f
        eq << Pel_ref - N_cell * x(4) * x(2);//, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

        const Scalar infinity = std::numeric_limits<Scalar>::infinity();
        lbx << 1, 85, 0, 0, 0.76;//, 650, -infinity, -infinity, -infinity, 650, -infinity, -infinity, -infinity, -infinity, -infinity, -infinity, -infinity, -infinity, -infinity, -infinity;
        ubx << 7, 200, 50, 40, infinity;//, 750, infinity, infinity, infinity, 790, 890, infinity, infinity, infinity, infinity, infinity, infinity, infinity, infinity, infinity;
    }
};

template <typename Solver>
void callback(void *solver_p)
{
    Solver& s = *static_cast<Solver*>(solver_p);

    Eigen::IOFormat fmt(Eigen::StreamPrecision, 0, ", ", ",", "[", "],");
    std::cout << s.info().iter << " " << s._x.transpose().format(fmt) << s._qp_solver.info().status << " " << s._cost << std::endl;
}

#if 0 // suspended, see issue #13
TEST(SQPTestCase, TestConstrainedRosenbrock) {
    using Solver = SQP<ConstrainedRosenbrock>;
    ConstrainedRosenbrock problem;
    Solver solver;
    Eigen::Vector2d x0, x;
    Eigen::Vector4d y0;
    y0.setZero();

    x0 << 0, 0;
    solver.settings().max_iter = 1000;
    solver.settings().line_search_max_iter = 10;
    // solver.settings().iteration_callback = callback<Solver>;
    solver.solve(problem, x0, y0);

    x = solver.primal_solution();

    std::cout << "iter " << solver.info().iter << std::endl;
    std::cout << "Solution " << x.transpose() << std::endl;

    EXPECT_TRUE(x.isApprox(problem.SOLUTION, 1e-2));
    EXPECT_LT(solver.info().iter, solver.settings().max_iter);
}
#endif

TEST(SQPTestCase, TestSOFCModel) {
    using Solver = SQP<SOFCModel>;
    SOFCModel problem;
    Solver solver;
    Eigen::Matrix<double, 5, 1> x;
    Eigen::Matrix<double, 5, 1> x0;
    x0 << 1.75, 150, 18, 30, 0.9;//, 500, 710, 711, 712, 713, 1000, 800, 700, 750, 800, 700, 750, 800, 700, 750;

    solver.settings().max_iter = 500;
    solver.settings().line_search_max_iter = 1;
    //solver._qp_solver.settings().adaptive_rho = true;
    solver._qp_solver.settings().verbose = false;
    solver._qp_solver.settings().max_iter = 1000;
    solver._qp_solver.settings().check_termination = 25;
    solver._qp_solver.settings().adaptive_rho_interval = 25;
    solver.settings().iteration_callback = callback<Solver>;
    solver.solve(problem, x0);

    x = solver.primal_solution();

    std::cout << "iter " << solver.info().iter << std::endl;
    std::cout << "Solution " << x.transpose() << std::endl;

    EXPECT_LT(solver.info().iter, solver.settings().max_iter);
}
/*
struct Rosenbrock : public ProblemBase<Rosenbrock,
                                       double,
                                       2,
                                       0,
                                       0>  {
    const Scalar a = 1;
    const Scalar b = 100;
    Eigen::Vector2d SOLUTION = {1.0, 1.0};

    template <typename DerivedA, typename DerivedB>
    void cost(const DerivedA& x, DerivedB &cst)
    {
        // (a-x)^2 + b*(y-x^2)^2
        cst = pow(a - x(0), 2) + b * pow(x(1) - pow(x(0), 2), 2);
    }

    template <typename A, typename B, typename C>
    void constraint(const A& x, B& eq, C& ineq, box_t& lbx, box_t& ubx)
    {
        // unconstrained
        const Scalar infinity = std::numeric_limits<Scalar>::infinity();
        lbx << -infinity, -infinity;
        ubx << infinity, infinity;
    }
};

struct SimpleNLP : ProblemBase<SimpleNLP, double, 2, 0, 2> {
    var_t SOLUTION = {1, 1};

    template <typename A, typename B>
    void cost(const A& x, B& cst)
    {
        cst = -x(0) -x(1);
    }

    template <typename A, typename B, typename C>
    void constraint(const A& x, B& eq, C& ineq, var_t& lbx, var_t& ubx)
    {
        const Scalar infinity = std::numeric_limits<Scalar>::infinity();
        ineq << 1 - x.squaredNorm(),
                  x.squaredNorm() - 2; // 1 <= x0^2 + x1^2 <= 2
        lbx << 0, 0; // x0 > 0 and x1 > 0
        ubx << infinity, infinity;
    }

};

TEST(SQPTestCase, TestSimpleNLP) {
    using Solver = SQP<SimpleNLP>;
    SimpleNLP problem;
    Solver solver;

    // feasible initial point
    Eigen::Vector2d x;
    Eigen::Vector2d x0 = {1.2, 0.1};
    Eigen::Vector4d y0;
    y0.setZero();

    solver.settings().max_iter = 100;
    solver.settings().line_search_max_iter = 4;
    solver.settings().iteration_callback = callback<Solver>;
    solver.solve(problem, x0, y0);

    x = solver.primal_solution();

    std::cout << "iter " << solver.info().iter << std::endl;
    std::cout << "qp_iter " << solver.info().qp_solver_iter << std::endl;
    std::cout << "Solution " << x.transpose() << std::endl;

    EXPECT_TRUE(x.isApprox(problem.SOLUTION, 1e-2));
    EXPECT_LT(solver.info().iter, solver.settings().max_iter);
}
*/
} // namespace sqp_test_autodiff


#include <omp.h>
// #define EIGEN_DONT_PARALLELIZE
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <algorithm>
#include <chrono>
#include <experimental/filesystem>
#include <fstream>
#include <iostream>
#include <limits>
#include <map>
#include <unordered_map>
#include <unordered_set>

#include "data_loader.h"

namespace fs = std::experimental::filesystem;
using DataType = float;
// using Eigen::ColMajor is Eigen restriction -  todense method always returns
// matrices in ColMajor order
using Matrix =
    Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;

using SparseMatrix = Eigen::SparseMatrix<DataType, Eigen::ColMajor>;

using DiagonalMatrix =
    Eigen::DiagonalMatrix<DataType, Eigen::Dynamic, Eigen::Dynamic>;

// Initialize matrix with random values and normalize them
Matrix InitialiseMatrix(Eigen::Index rows, Eigen::Index cols) {
  Matrix mat = Matrix::Random(rows, cols).array().abs();
  auto row_sums = mat.rowwise().sum();
  mat.array().colwise() /= row_sums.array();
  return mat;
}

Matrix RatingsPredictions(const Matrix& x, const Matrix& y) {
  return x * y.transpose();
}

DataType CalculateWeightedMse(const Matrix& x,
                              const Matrix& y,
                              const SparseMatrix& p,
                              const SparseMatrix& ratings_matrix,
                              DataType alpha) {
  Matrix c(ratings_matrix);
  c.array() *= alpha;
  c.array() += 1.0;

  Matrix diff(p - RatingsPredictions(x, y));
  diff = diff.array().pow(2.f);

  Matrix weighted_diff = c.array() * diff.array();
  return weighted_diff.array().mean();
}

void PrintRecommendations(const Matrix& ratings_matrix,
                          const Matrix& ratings_matrix_pred,
                          const std::vector<std::string>& movie_titles) {
  // auto m = ratings_matrix.rows();
  auto n = ratings_matrix.cols();
  std::vector<std::string> liked;
  std::vector<std::string> recommended;
  for (Eigen::Index u = 0; u < 5; ++u) {
    for (Eigen::Index i = 0; i < n; ++i) {
      DataType orig_value = ratings_matrix(u, i);
      if (orig_value >= 3.f) {
        liked.push_back(movie_titles[static_cast<size_t>(i)]);
      }
      DataType pred_value = ratings_matrix_pred(u, i);
      if (pred_value >= 0.8f && orig_value < 1.f) {
        recommended.push_back(movie_titles[static_cast<size_t>(i)]);
      }
    }
    std::cout << "\nUser " << u << " liked :";
    for (auto& l : liked) {
      std::cout << l << "; ";
    }
    std::cout << "\nUser " << u << " recommended :";
    for (auto& r : recommended) {
      std::cout << r << "; ";
    }
    std::cout << std::endl;
    liked.clear();
    recommended.clear();
  }
}

int main(int argc, char** argv) {
  if (argc == 2) {
    Eigen::initParallel();
    auto root_path = fs::path(argv[1]);
    if (fs::exists(root_path)) {
      SparseMatrix ratings_matrix;  // user-item ratings
      SparseMatrix p;               // binary variables
      std::vector<std::string> movie_titles;
      {
        std::cout << "Data loading .." << std::endl;
        // load data
        auto movies_file = root_path / "movies.csv";
        auto movies = LoadMovies(movies_file);

        auto ratings_file = root_path / "ratings.csv";
        auto ratings = LoadRatings(ratings_file);

        std::cout << "Data loaded" << std::endl;

        // merge movies and users
        std::cout << "Data merging..." << std::endl;
        // fill matrix
        ratings_matrix.resize(static_cast<Eigen::Index>(ratings.size()),
                              static_cast<Eigen::Index>(movies.size()));
        ratings_matrix.setZero();
        p.resize(ratings_matrix.rows(), ratings_matrix.cols());
        p.setZero();

        movie_titles.resize(movies.size());

        Eigen::Index user_idx = 0;
        for (auto& r : ratings) {
          for (auto& m : r.second) {
            auto mi = movies.find(m.first);
            Eigen::Index movie_idx = std::distance(movies.begin(), mi);
            movie_titles[static_cast<size_t>(movie_idx)] = mi->second;
            ratings_matrix.insert(user_idx, movie_idx) =
                static_cast<DataType>(m.second);
            p.insert(user_idx, movie_idx) = 1.0;
          }
          ++user_idx;
        }
        ratings_matrix.makeCompressed();
        std::cout << "Data merged" << std::endl;
      }

      // prepare for learning
      auto m = ratings_matrix.rows();
      auto n = ratings_matrix.cols();

      std::cout << "Users " << m << " Movies " << n << std ::endl;

      Eigen::Index n_factors = 100;
      auto y = InitialiseMatrix(n, n_factors);
      auto x = InitialiseMatrix(m, n_factors);

      // Test initialization
      DataType alpha = 40.f;  // confidence level parameter
      auto w_mse = CalculateWeightedMse(x, y, p, ratings_matrix, alpha);
      std::cout << "Initial weighted mse " << w_mse << std::endl;

      // Precalculate regularization term
      DataType reg_lambda = 0.1f;
      SparseMatrix reg =
          (reg_lambda * Matrix::Identity(n_factors, n_factors)).sparseView();

      // Define diagonal identity terms
      SparseMatrix user_diag = -1 * Matrix::Identity(n, n).sparseView();
      SparseMatrix item_diag = -1 * Matrix::Identity(m, m).sparseView();

      // define weights
      std::cout << "Calculate weights ..." << std::endl;
      std::vector<DiagonalMatrix> user_weights(static_cast<size_t>(m));
      std::vector<DiagonalMatrix> item_weights(static_cast<size_t>(n));
      {
        Matrix weights(ratings_matrix);
        weights.array() *= alpha;
        weights.array() += 1;

        for (Eigen::Index i = 0; i < m; ++i) {
          user_weights[static_cast<size_t>(i)] = weights.row(i).asDiagonal();
        }
        for (Eigen::Index i = 0; i < n; ++i) {
          item_weights[static_cast<size_t>(i)] = weights.col(i).asDiagonal();
        }
      }

      // learning loop
      size_t n_iterations = 5;
      std::cout << "Start learning ..." << std::endl;
      // omp_set_num_threads(4);
      for (size_t k = 0; k < n_iterations; ++k) {
        auto start_time = std::chrono::steady_clock::now();
        auto yt = y.transpose();
        auto yty = yt * y;

#pragma omp parallel
        {
          Matrix diff;
          Matrix ytcuy;
          Matrix a, b, update_y;
#pragma omp for private(diff, ytcuy, a, b, update_y)
          for (size_t i = 0; i < static_cast<size_t>(m); ++i) {
            diff = user_diag;
            diff += user_weights[i];
            ytcuy = yty + yt * diff * y;
            auto p_val = p.row(static_cast<Eigen::Index>(i)).transpose();

            a = ytcuy + reg;
            b = yt * user_weights[i] * p_val;

            update_y = a.colPivHouseholderQr().solve(b);
            x.row(static_cast<Eigen::Index>(i)) = update_y.transpose();
          }
        }

        auto xt = x.transpose();
        auto xtx = xt * x;

#pragma omp parallel
        {
          Matrix diff;
          Matrix xtcux;
          Matrix a, b, update_x;
#pragma omp for private(diff, xtcux, a, b, update_x)
          for (size_t i = 0; i < static_cast<size_t>(n); ++i) {
            diff = item_diag;
            diff += item_weights[i];
            xtcux = xtx + xt * diff * x;
            auto p_val = p.col(static_cast<Eigen::Index>(i));

            a = xtcux + reg;
            b = xt * item_weights[i] * p_val;

            update_x = a.colPivHouseholderQr().solve(b);
            y.row(static_cast<Eigen::Index>(i)) = update_x.transpose();
          }
        }

        w_mse = CalculateWeightedMse(x, y, p, ratings_matrix, alpha);
        auto finish_time = std::chrono::steady_clock::now();
        double elapsed_seconds =
            std::chrono::duration_cast<std::chrono::duration<double>>(
                finish_time - start_time)
                .count();

        std::cout << "Initeration " << k << " weighted mse " << w_mse
                  << " time " << elapsed_seconds << std::endl;
      }
      std::cout << "Learning done" << std::endl;

      PrintRecommendations(ratings_matrix, RatingsPredictions(x, y),
                           movie_titles);

      return 0;
    }
  }

  std::cout << "please specify data set directory\n";
  return 0;
};


//          Copyright Jean Pierre Cimalando 2018.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <jsl/math>
#include <boost/predef/architecture.h>
#include <cmath>

namespace jsl {

template <class R>
inline R clamp(R x, R min, R max)
{
  x = (x < min) ? min : x;
  x = (x > max) ? max : x;
  return x;
}

template <class R>
inline R square(R x)
{
    return x * x;
}

template <class R>
inline R cube(R x)
{
    return x * x * x;
}

template <class R>
R sinc(R x)
{
    return (x == 0) ? 1 : (std::sin(x) / x);
}

template <class R>
R binom(unsigned n, unsigned k)
{
  R r = 1;
  for (unsigned i = 1; i <= k; ++i)
    r *= (n + 1 - i) / (R)i;
  return r;
}

template <class R>
void poly(gsl::span<const R> z, gsl::span<R> p)
{
    Expects(p.size() == z.size() + 1);
    size_t n = z.size();
    p[0] = 1;
    for (size_t j = 0; j < n; ++j)
        p[j + 1] = 0;
    for (size_t j = 0; j < n; ++j)
        for (size_t k = j + 1; k-- > 0;)
            p[k + 1] -= z[j] * p[k];
}

template <class R>
R polyval(gsl::span<const R> p, R x)
{
    R y = 0;
    R xi = 1;
    for (size_t i = 0, n = p.size(); i < n; ++i) {
        y += xi * p[i];
        xi *= x;
    }
    return y;
}

#if BOOST_ARCH_X86_32 || BOOST_ARCH_X86_64
struct denormal_disabler {
    denormal_disabler() noexcept
    {
        int csr = get_csr();
        csr_ = csr;
        set_csr(csr | 0x8040);
    }

    ~denormal_disabler() noexcept
    {
        set_csr(csr_);
    }

private:
    int csr_ = 0;

    static int get_csr() noexcept {
        int csr;
        asm volatile("stmxcsr %0" : "=m"(csr));
        return csr;
    }

    static void set_csr(int csr) noexcept {
        asm volatile("ldmxcsr %0" : : "m"(csr));
    }
};
#elif BOOST_ARCH_ARM
struct denormal_disabler {
    denormal_disabler() noexcept
    {
        int fpcsr = get_fpcsr();
        fpcsr_ = fpcsr;
        set_fpcsr(fpcsr | (1 << 24));
    }

    ~denormal_disabler() noexcept
    {
        set_fpcsr(fpcsr_);
    }

private:
    int fpcsr_ = 0;

    static int get_fpcsr() noexcept {
        int fpcsr;
        asm volatile("mrs %[fpcsr], FPCR" : [fpcsr] "=r"(fpcsr));
        return fpcsr;
    }

    static void set_fpcsr(int fpcsr) noexcept {
        asm volatile("msr FPCR, %[fpcsr]" : : [fpcsr]"r"(fpcsr));
    }
};
#else
struct denormal_disabler {
    denormal_disabler() noexcept {}
    ~denormal_disabler() noexcept {}
};
#endif

namespace ilog2_detail {

template <class T>
struct ilog2 {
    T operator()(T value)
    {
        T l = 0;
        while((value >> l) > 1)
            ++l;
        return l;
    }
};

#if defined(__GNUC__)
template <>
struct ilog2<unsigned> {
    unsigned operator()(unsigned value)
    {
        return sizeof(unsigned) * 8 - __builtin_clz(value) - 1;
    }
};

template <>
struct ilog2<unsigned long> {
    unsigned long operator()(unsigned long value)
    {
        return sizeof(unsigned long) * 8 - __builtin_clzl(value) - 1;
    }
};

template <>
struct ilog2<unsigned long long> {
    unsigned long long operator()(unsigned long long value)
    {
        return sizeof(unsigned long long) * 8 - __builtin_clzll(value) - 1;
    }
};
#endif

}  // namespace ilog2_detail

template <class T>
T ilog2(T value)
{
    ilog2_detail::ilog2<T> fn;
    return fn(value);
}

}  // namespace jsl


#pragma once
#include "Optimizer.hpp"

#include <Eigen/Dense>
#include <array>
#include <limits>
#include <nlohmann/json.hpp>

namespace yavque
{
class AdaMax : public Optimizer
{
public:
	static constexpr std::array<double, 3> DEFAULT_PARAMS = {0.002, 0.9, 0.999};

private:
	const double alpha_;
	const double beta1_;
	const double beta2_;

	int t_ = 0;

	Eigen::VectorXd m_;
	Eigen::VectorXd u_;

public:
	explicit AdaMax(double alpha = DEFAULT_PARAMS[0], double beta1 = DEFAULT_PARAMS[1],
	                double beta2 = DEFAULT_PARAMS[2])
		: alpha_(alpha), beta1_(beta1), beta2_(beta2)
	{
	}

	explicit AdaMax(const nlohmann::json& params)
		: alpha_(params.value("alpha", DEFAULT_PARAMS[0])),
		  beta1_(params.value("beta1", DEFAULT_PARAMS[1])),
		  beta2_(params.value("beta2", DEFAULT_PARAMS[2]))
	{
	}

	static nlohmann::json defaultParams()
	{
		return nlohmann::json{{"name", "AdaMax"},
		                      {"alhpa", DEFAULT_PARAMS[0]},
		                      {"beta1", DEFAULT_PARAMS[1]},
		                      {"beta2", DEFAULT_PARAMS[2]}};
	}

	[[nodiscard]] nlohmann::json desc() const override
	{
		return nlohmann::json{{"name", "AdaMax"},
		                      {"alhpa", alpha_},
		                      {"beta1", beta1_},
		                      {"beta2", beta2_}};
	}

	Eigen::VectorXd getUpdate(const Eigen::VectorXd& grad) override
	{
		using std::pow;
		if(t_ == 0)
		{
			m_ = Eigen::VectorXd::Zero(grad.rows());
			u_ = Eigen::VectorXd::Zero(grad.rows());
		}
		++t_;
		m_ *= beta1_;
		m_ += (1.0 - beta1_) * grad;

		u_ *= beta2_;
		u_ = u_.cwiseMax(grad.cwiseAbs());
		u_ = u_.cwiseMax(std::numeric_limits<double>::min());

		return -(alpha_ / (1 - pow(beta1_, t_))) * m_.cwiseQuotient(u_);
	}
};
} // namespace yavque


#ifndef SOFTMAX_H
#define SOFTMAX_H

#include "../../ml/utility/gradient_checking.hpp"
#include "../../eigen/eigen.hpp"

#include <opencv2/core.hpp>

#include <Eigen/Dense>

#include <iostream>
#include <limits>
#include <map>
#include <random>
#include <set>
#include <vector>

/*! \file softmax.hpp
    \brief implement the algorithm--softmax regression based on\n
    the description of UFLDL, these codes are develop based\n
    on the example on the website(http://eric-yuan.me/softmax-regression-cv/#comment-8781).
*/

/*!
 *  \addtogroup ocv
 *  @{
 */
namespace ocv{

/*!
 *  \addtogroup ml
 *  @{
 */
namespace ml{

template<typename T = double>
class softmax
{
public:
    static_assert(std::is_floating_point<T>::value,
                  "T should be floating point");

    using EigenMat = eigen::MatRowMajor<T>;

    softmax();

    /**
     * @brief get the weight of softmax
     * @return weight of softmax
     */
    EigenMat const& get_weight() const
    {
        return weight_;
    }

    std::vector<int> const& batch_predicts
    (Eigen::Ref<const EigenMat> const &input);
    std::vector<int> const& batch_predicts(cv::Mat const &input);
    int predict(Eigen::Ref<const EigenMat> const &input);
    int predict(cv::Mat const &input);

    /**
     * @brief Set the batch size of mini-batch
     * @param batch_size batch size of mini-batch,default\n
     * value is 100, if the train data is smaller than\n
     * the batch size, the batch size will be same as the\n
     * batch size
     */
    void set_batch_size(int batch_size)
    {
        params_.batch_size_ = batch_size;
    }

    /**
     * @brief softmax::set_epsillon
     * @param epsillon The desired accuracy or change\n
     *  in parameters at which the iterative algorithm stops.\n
     *  Default value is 1e-5
     */
    void set_epsillon(double epsillon)
    {
        params_.epsillon_ = epsillon;
    }

    /**
     * @brief Setup the lambda
     * @param lambda the lambda value which determine the effect\n
     * of penalizes term.Default value is 2.0
     */
    void set_lambda(double lambda)
    {
        params_.lambda_ = lambda;
    }

    /**
     * @brief Set the learning rate
     * @param lrate The larger the learning rate, the faster\n
     * the convergence speed, but larger value may cause divergence too.\n
     * Default value is 0.2
     */
    void softmax::set_learning_rate(double lrate)
    {
        params_.lrate_ = lrate;
    }

    /**
     * @brief Set max iterateration times
     * @param max_iter max iteration time, default value is 10000
     */
    void softmax::set_max_iter(int max_iter)
    {
        params_.max_iter_ = max_iter;
    }

    void read(const std::string &file);

    void train(const Eigen::Ref<const EigenMat> &train,
               const std::vector<int> &labels);

    void write(const std::string &file) const;

private:
    double compute_cost(Eigen::Ref<const EigenMat> const &train,
                        Eigen::Ref<const EigenMat> const &weight,
                        Eigen::Ref<const EigenMat> const &ground_truth);

    void compute_gradient(Eigen::Ref<const EigenMat> const &train,
                          Eigen::Ref<const EigenMat> const &weight,
                          Eigen::Ref<const EigenMat> const &ground_truth);

    void compute_hypothesis(Eigen::Ref<const EigenMat> const &train,
                            Eigen::Ref<const EigenMat> const &weight);

    int get_batch_size(int sample_size) const
    {
        return std::min(sample_size, params_.batch_size_);
    }

    EigenMat get_ground_truth(int NumClass,
                              int samples_size,
                              std::map<int, int> const &unique_labels,
                              std::vector<int> const &labels) const;
    std::map<int, int> softmax::
    get_unique_labels(const std::vector<int> &labels) const;

    void gradient_check()
    {
        std::vector<int> const Labels{0, 1, 2, 0};
        auto const UniqueLabels = get_unique_labels(Labels);
        auto const NumClass = UniqueLabels.size();
        EigenMat const Train = EigenMat::Random(10, 2);
        weight_ = EigenMat::Random(NumClass, Train.rows());
        grad_ = EigenMat::Zero(NumClass, Train.rows());
        int const TrainCols = static_cast<int>(Train.cols());
        EigenMat const GroundTruth = get_ground_truth(NumClass, TrainCols,
                                                      UniqueLabels,
                                                      Labels);
        gradient_checking gc;
        auto func = [&](EigenMat &theta)->double
        {
            return compute_cost(Train, theta, GroundTruth);
        };

        EigenMat const WeightBuffer = weight_;
        EigenMat const Gradient =
                gc.compute_gradient(weight_, func);

        compute_cost(Train, WeightBuffer, GroundTruth);
        compute_gradient(Train, WeightBuffer, GroundTruth);

        std::cout<<std::boolalpha<<"gradient checking pass : "
                <<gc.compare_gradient(grad_, Gradient)<<"\n";//*/
    }

    struct criteria
    {
        criteria();
        int batch_size_;
        double cost_;
        double epsillon_;
        double lambda_;
        double lrate_;
        int max_iter_;
    };

    EigenMat hypothesis_;
    EigenMat grad_;
    EigenMat max_exp_power_;
    criteria params_;
    std::vector<int> predicts_;
    EigenMat probability_;
    EigenMat weight_;
    EigenMat weight_sum_;
};

template<typename T>
softmax<T>::softmax()
{

}

/**
 *@brief Predicts the response for input samples(multiple samples)
 *@param input input data for prediction, each col associate\n
 * with one sample
 *@return Output prediction responses for corresponding samples
 *@pre rows are the features, cols are the corresponding samples.\n
 * This function can predict multiple samples
 */
template<typename T>
std::vector<int> const& softmax<T>::
batch_predicts(Eigen::Ref<const EigenMat> const &input)
{
    predicts_.resize(input.cols());
    compute_hypothesis(input, weight_);
    for(size_t i = 0; i != predicts_.size(); ++i){
        probability_ = (hypothesis_.col(i) *
                        input.col(i).transpose()).
                rowwise().sum();
        EigenMat::Index max_row = 0, max_col = 0;
        probability_.maxCoeff(&max_row, &max_col);
        predicts_[i] = static_cast<int>(max_row);
    }

    return predicts_;
}

/**
 *@brief Predicts the response for input samples(multiple samples)
 *@param input input data for prediction, each col associate\n
 * with one sample
 *@return Output prediction responses for corresponding samples
 *@pre rows are the features, cols are the corresponding samples.\n
 * This function can predict multiple samples
 */
template<typename T>
std::vector<int> const& softmax<T>::
batch_predicts(cv::Mat const &input)
{
    Eigen::Map<EigenMat> const Map(reinterpret_cast<*>(input.data),
                                   input.rows,
                                   input.step / sizeof(T));
    return batch_predicts(Map.block(0, 0, input.rows, input.cols));
}

/**
 *@brief Predicts the response for input sample(one sample)
 *@param input input data for prediction, each col associate\n
 * with one sample
 *@return Output prediction responses for corresponding sample
 *@pre rows are the features, col is the corresponding sample.\n
 * This function can predict one sample only
 */
template<typename T>
int softmax<T>::predict(Eigen::Ref<const EigenMat> const &input)
{
    CV_Assert(input.cols() == 1);
    compute_hypothesis(input, weight_);
    probability_ = (hypothesis_ * input.transpose()).
            rowwise().sum();
    EigenMat::Index max_row = 0, max_col = 0;
    probability_.maxCoeff(&max_row, &max_col);

    return max_row;
}

/**
 *@brief Predicts the response for input sample(one sample)
 *@param input input data for prediction
 *@return Output prediction responses for corresponding sample
 *@pre rows are the features, col is the corresponding sample.\n
 * This function can predict one sample only
 */
template<typename T>
int softmax<T>::predict(cv::Mat const &input)
{
    Eigen::Map<EigenMat> const Map(reinterpret_cast<*>(input.data),
                                   input.rows,
                                   input.step / sizeof(T));
    return predict(Map.block(0, 0, input.rows, input.cols));
}

/**
 * @brief read the training result into the data
 * @param file the name of the file
 */
template<typename T>
void softmax<T>::read(const std::string &file)
{
    cv::FileStorage in(file, cv::FileStorage::READ);

    in["batch_size"]>>params_.batch_size_;
    in["cost_"]>>params_.cost_;
    in["epsillon_"]>>params_.epsillon_;
    in["lambda_"]>>params_.lambda_;
    in["lrate_"]>>params_.lrate_;
    in["max_iter_"]>>params_.max_iter_;
    cv::Mat weight;
    in["weight"]>>weight;
    eigen::cv2eigen_cpy(weight, weight_);
}

/**
 * @brief Train the input data by softmax algorithm
 * @param train Training data, input contains one\n
 *  training example per column
 * @param labels The label of each training example
 */
template<typename T>
void softmax<T>::train(const Eigen::Ref<const EigenMat> &train,
                       const std::vector<int> &labels)
{
#ifdef OCV_TEST_SOFTMAX
    gradient_check();
#endif

    auto const UniqueLabels = get_unique_labels(labels);
    auto const NumClass = UniqueLabels.size();
    weight_ = EigenMat::Random(NumClass, train.rows());
    grad_ = EigenMat::Zero(NumClass, train.rows());
    auto const TrainCols = static_cast<int>(train.cols());
    EigenMat const GroundTruth = get_ground_truth(static_cast<int>(NumClass),
                                                  TrainCols,
                                                  UniqueLabels,
                                                  labels);

    std::random_device rd;
    std::default_random_engine re(rd());
    int const Batch = (get_batch_size(TrainCols));
    int const RandomSize = TrainCols != Batch ?
                TrainCols - Batch - 1 : 0;
    std::uniform_int_distribution<int>
            uni_int(0, RandomSize);
    for(size_t i = 0; i != params_.max_iter_; ++i){
        auto const Cols = uni_int(re);
        auto const &TrainBlock =
                train.block(0, Cols, train.rows(), Batch);
        auto const &GTBlock =
                GroundTruth.block(0, Cols, NumClass, Batch);
        auto const Cost = compute_cost(TrainBlock, weight_, GTBlock);
        if(std::abs(params_.cost_ - Cost) < params_.epsillon_ ||
                Cost < 0){
            break;
        }
        params_.cost_ = Cost;
        compute_gradient(TrainBlock, weight_, GTBlock);
        weight_.array() -= grad_.array() * params_.lrate_;//*/
    }
}

template<typename T>
void softmax<T>::write(const std::string &file) const
{
    cv::FileStorage out(file, cv::FileStorage::WRITE);

    out<<"batch_size"<<params_.batch_size_;
    out<<"cost_"<<params_.cost_;
    out<<"epsillon_"<<params_.epsillon_;
    out<<"lambda_"<<params_.lambda_;
    out<<"lrate_"<<params_.lrate_;
    out<<"max_iter_"<<params_.max_iter_;
    cv::Mat const Weight = eigen::eigen2cv_ref(weight_);
    out<<"weight"<<Weight;
}

template<typename T>
double softmax<T>::compute_cost(const Eigen::Ref<const EigenMat> &train,
                                const Eigen::Ref<const EigenMat> &weight,
                                const Eigen::Ref<const EigenMat> &ground_truth)
{
    compute_hypothesis(train, weight);
    double const NSamples = static_cast<double>(train.cols());
    return  -1.0 * (hypothesis_.array().log() *
                    ground_truth.array()).sum() / NSamples +
            weight.array().pow(2.0).sum() * params_.lambda_ / 2.0;
}

template<typename T>
void softmax<T>::compute_gradient(Eigen::Ref<const EigenMat> const &train,
                                  Eigen::Ref<const EigenMat> const &weight,
                                  Eigen::Ref<const EigenMat> const &ground_truth)
{
    grad_.noalias() =
            (ground_truth.array() - hypothesis_.array())
            .matrix() * train.transpose();
    auto const NSamples = static_cast<double>(train.cols());
    grad_.array() = grad_.array() / -NSamples +
            params_.lambda_ * weight.array();
}

template<typename T>
void softmax<T>::compute_hypothesis(Eigen::Ref<const EigenMat> const &train,
                                    Eigen::Ref<const EigenMat> const &weight)
{
    hypothesis_.noalias() = weight * train;
    max_exp_power_ = hypothesis_.colwise().maxCoeff();
    for(size_t i = 0; i != hypothesis_.cols(); ++i){
        hypothesis_.col(i).array() -= max_exp_power_(0, i);
    }

    hypothesis_ = hypothesis_.array().exp();
    weight_sum_ = hypothesis_.array().colwise().sum();
    for(size_t i = 0; i != hypothesis_.cols(); ++i){
        if(weight_sum_(0, i) != T(0)){
            hypothesis_.col(i) /= weight_sum_(0, i);
        }
    }
    hypothesis_ = (hypothesis_.array() != 0 ).
            select(hypothesis_, T(0.1));
}

template<typename T>
typename softmax<T>::EigenMat softmax<T>::
get_ground_truth(int NumClass, int samples_size,
                 std::map<int, int> const &unique_labels,
                 std::vector<int> const &labels) const
{
    EigenMat ground_truth = EigenMat::Zero(NumClass, samples_size);
    for(size_t i = 0; i != ground_truth.cols(); ++i){
        auto it = unique_labels.find(labels[i]);
        if(it != std::end(unique_labels)){
            ground_truth(it->second, i) = 1;
        }
    }

    return ground_truth;
}

template<typename T>
std::map<int, int> softmax<T>::
get_unique_labels(const std::vector<int> &labels) const
{
    std::set<int> const UniqueLabels(std::begin(labels),
                                     std::end(labels));
    std::map<int, int> result;
    int i = 0;
    for(auto it = std::begin(UniqueLabels);
        it != std::end(UniqueLabels); ++it){
        if(result.find(*it) ==
                std::end(result)){
            result.emplace(*it, i++);
        }
    }

    return result;
}

template<typename T>
softmax<T>::criteria::criteria() :
    batch_size_{100},
    cost_{std::numeric_limits<double>::max()},
    epsillon_{1e-5},
    lambda_{2.0},
    lrate_{0.2},
    max_iter_{10000}
{

}

} /*! @} End of Doxygen Groups*/

} /*! @} End of Doxygen Groups*/

#endif // SOFTMAX_H


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

// @sect3{Include files}
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/function.h>
#include <deal.II/base/utilities.h>

#include <deal.II/lac/block_vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/block_sparse_matrix.h>
#include <deal.II/lac/block_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_gmres.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_tools.h>
#include <deal.II/grid/grid_refinement.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_renumbering.h>
#include <deal.II/dofs/dof_tools.h>

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

#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>

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

#include <deal.II/lac/sparse_ilu.h>
#include <deal.II/grid/grid_out.h>

// 我们需要包括以下文件来做计时。

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

// 这包括我们使用几何多网格所需的文件

#include <deal.II/multigrid/multigrid.h>
#include <deal.II/multigrid/mg_transfer.h>
#include <deal.II/multigrid/mg_tools.h>
#include <deal.II/multigrid/mg_coarse.h>
#include <deal.II/multigrid/mg_smoother.h>
#include <deal.II/multigrid/mg_matrix.h>

#include <iostream>
#include <fstream>

namespace Step56
{
  using namespace dealii;

// 为了便于在所使用的不同求解器之间进行切换，我们声明了一个枚举，可以作为参数传递给主类的构造函数。

  enum class SolverType
  {
    FGMRES_ILU,
    FGMRES_GMG,
    UMFPACK
  };
// @sect3{Functions for Solution and Righthand side}

//Solution类用于定义边界条件和计算数值解的误差。请注意，我们需要定义数值和梯度，以便计算L2和H1误差。在这里，我们决定使用模板的特殊化来分离2D和3D的实现。

// 请注意，前几个分量是速度分量，最后一个分量是压力。

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

  template <>
  double Solution<2>::value(const Point<2> &   p,
                            const unsigned int component) const
  {
    Assert(component <= 2 + 1, ExcIndexRange(component, 0, 2 + 1));

    using numbers::PI;
    const double x = p(0);
    const double y = p(1);

    if (component == 0)
      return sin(PI * x);
    if (component == 1)
      return -PI * y * cos(PI * x);
    if (component == 2)
      return sin(PI * x) * cos(PI * y);

    return 0;
  }

  template <>
  double Solution<3>::value(const Point<3> &   p,
                            const unsigned int component) const
  {
    Assert(component <= 3 + 1, ExcIndexRange(component, 0, 3 + 1));

    using numbers::PI;
    const double x = p(0);
    const double y = p(1);
    const double z = p(2);

    if (component == 0)
      return 2.0 * sin(PI * x);
    if (component == 1)
      return -PI * y * cos(PI * x);
    if (component == 2)
      return -PI * z * cos(PI * x);
    if (component == 3)
      return sin(PI * x) * cos(PI * y) * sin(PI * z);

    return 0;
  }

// 注意，对于梯度，我们需要返回一个Tensor<1,dim>。

  template <>
  Tensor<1, 2> Solution<2>::gradient(const Point<2> &   p,
                                     const unsigned int component) const
  {
    Assert(component <= 2, ExcIndexRange(component, 0, 2 + 1));

    using numbers::PI;
    const double x = p(0);
    const double y = p(1);

    Tensor<1, 2> return_value;
    if (component == 0)
      {
        return_value[0] = PI * cos(PI * x);
        return_value[1] = 0.0;
      }
    else if (component == 1)
      {
        return_value[0] = y * PI * PI * sin(PI * x);
        return_value[1] = -PI * cos(PI * x);
      }
    else if (component == 2)
      {
        return_value[0] = PI * cos(PI * x) * cos(PI * y);
        return_value[1] = -PI * sin(PI * x) * sin(PI * y);
      }

    return return_value;
  }

  template <>
  Tensor<1, 3> Solution<3>::gradient(const Point<3> &   p,
                                     const unsigned int component) const
  {
    Assert(component <= 3, ExcIndexRange(component, 0, 3 + 1));

    using numbers::PI;
    const double x = p(0);
    const double y = p(1);
    const double z = p(2);

    Tensor<1, 3> return_value;
    if (component == 0)
      {
        return_value[0] = 2 * PI * cos(PI * x);
        return_value[1] = 0.0;
        return_value[2] = 0.0;
      }
    else if (component == 1)
      {
        return_value[0] = y * PI * PI * sin(PI * x);
        return_value[1] = -PI * cos(PI * x);
        return_value[2] = 0.0;
      }
    else if (component == 2)
      {
        return_value[0] = z * PI * PI * sin(PI * x);
        return_value[1] = 0.0;
        return_value[2] = -PI * cos(PI * x);
      }
    else if (component == 3)
      {
        return_value[0] = PI * cos(PI * x) * cos(PI * y) * sin(PI * z);
        return_value[1] = -PI * sin(PI * x) * sin(PI * y) * sin(PI * z);
        return_value[2] = PI * sin(PI * x) * cos(PI * y) * cos(PI * z);
      }

    return return_value;
  }

// 实现  $f$  。更多信息请参见介绍。

  template <int dim>
  class RightHandSide : public Function<dim>
  {
  public:
    RightHandSide()
      : Function<dim>(dim + 1)
    {}

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

  template <>
  double RightHandSide<2>::value(const Point<2> &   p,
                                 const unsigned int component) const
  {
    Assert(component <= 2, ExcIndexRange(component, 0, 2 + 1));

    using numbers::PI;
    double x = p(0);
    double y = p(1);
    if (component == 0)
      return PI * PI * sin(PI * x) + PI * cos(PI * x) * cos(PI * y);
    if (component == 1)
      return -PI * PI * PI * y * cos(PI * x) - PI * sin(PI * y) * sin(PI * x);
    if (component == 2)
      return 0;

    return 0;
  }

  template <>
  double RightHandSide<3>::value(const Point<3> &   p,
                                 const unsigned int component) const
  {
    Assert(component <= 3, ExcIndexRange(component, 0, 3 + 1));

    using numbers::PI;
    double x = p(0);
    double y = p(1);
    double z = p(2);
    if (component == 0)
      return 2 * PI * PI * sin(PI * x) +
             PI * cos(PI * x) * cos(PI * y) * sin(PI * z);
    if (component == 1)
      return -PI * PI * PI * y * cos(PI * x) +
             PI * (-1) * sin(PI * y) * sin(PI * x) * sin(PI * z);
    if (component == 2)
      return -PI * PI * PI * z * cos(PI * x) +
             PI * cos(PI * z) * sin(PI * x) * cos(PI * y);
    if (component == 3)
      return 0;

    return 0;
  }

//  @sect3{ASPECT BlockSchurPreconditioner}

// 在下文中，我们将实现一个预处理程序，它扩展了  step-22  的结果部分所讨论的想法。具体来说，我们1.使用一个上块三角的预处理器，因为我们想使用右预处理。2.可选择允许对速度块使用内部求解器，而不是使用单一的预处理程序。3.不使用InverseMatrix，而是明确地调用SolverCG。这种方法也用于ASPECT代码（见https:aspect.geodynamics.org），该代码在模拟地幔对流的背景下求解斯托克斯方程，该代码已被用于解决成千上万个处理器上的问题。

//构造函数中的bool标志 @p do_solve_A 允许我们对速度块应用一次预处理，或者使用内部迭代求解器来代替更精确的近似。

// 注意我们是如何跟踪内部迭代的总和（预处理程序的应用）的。

  template <class PreconditionerAType, class PreconditionerSType>
  class BlockSchurPreconditioner : public Subscriptor
  {
  public:
    BlockSchurPreconditioner(
      const BlockSparseMatrix<double> &system_matrix,
      const SparseMatrix<double> &     schur_complement_matrix,
      const PreconditionerAType &      preconditioner_A,
      const PreconditionerSType &      preconditioner_S,
      const bool                       do_solve_A);

    void vmult(BlockVector<double> &dst, const BlockVector<double> &src) const;

    mutable unsigned int n_iterations_A;
    mutable unsigned int n_iterations_S;

  private:
    const BlockSparseMatrix<double> &system_matrix;
    const SparseMatrix<double> &     schur_complement_matrix;
    const PreconditionerAType &      preconditioner_A;
    const PreconditionerSType &      preconditioner_S;

    const bool do_solve_A;
  };

  template <class PreconditionerAType, class PreconditionerSType>
  BlockSchurPreconditioner<PreconditionerAType, PreconditionerSType>::
    BlockSchurPreconditioner(
      const BlockSparseMatrix<double> &system_matrix,
      const SparseMatrix<double> &     schur_complement_matrix,
      const PreconditionerAType &      preconditioner_A,
      const PreconditionerSType &      preconditioner_S,
      const bool                       do_solve_A)
    : n_iterations_A(0)
    , n_iterations_S(0)
    , system_matrix(system_matrix)
    , schur_complement_matrix(schur_complement_matrix)
    , preconditioner_A(preconditioner_A)
    , preconditioner_S(preconditioner_S)
    , do_solve_A(do_solve_A)
  {}

  template <class PreconditionerAType, class PreconditionerSType>
  void
  BlockSchurPreconditioner<PreconditionerAType, PreconditionerSType>::vmult(
    BlockVector<double> &      dst,
    const BlockVector<double> &src) const
  {
    Vector<double> utmp(src.block(0));

// 首先用S的近似值求解

    {
      SolverControl solver_control(1000, 1e-6 * src.block(1).l2_norm());
      SolverCG<Vector<double>> cg(solver_control);

      dst.block(1) = 0.0;
      cg.solve(schur_complement_matrix,
               dst.block(1),
               src.block(1),
               preconditioner_S);

      n_iterations_S += solver_control.last_step();
      dst.block(1) *= -1.0;
    }

// 第二，应用右上方的块（B^T

    {
      system_matrix.block(0, 1).vmult(utmp, dst.block(1));
      utmp *= -1.0;
      utmp += src.block(0);
    }

// 最后，要么用左上角的块求解，要么只应用一个预设条件器扫频

    if (do_solve_A == true)
      {
        SolverControl            solver_control(10000, utmp.l2_norm() * 1e-4);
        SolverCG<Vector<double>> cg(solver_control);

        dst.block(0) = 0.0;
        cg.solve(system_matrix.block(0, 0),
                 dst.block(0),
                 utmp,
                 preconditioner_A);

        n_iterations_A += solver_control.last_step();
      }
    else
      {
        preconditioner_A.vmult(dst.block(0), utmp);
        n_iterations_A += 1;
      }
  }
// @sect3{The StokesProblem class}

// 这是该问题的主类。

  template <int dim>
  class StokesProblem
  {
  public:
    StokesProblem(const unsigned int pressure_degree,
                  const SolverType   solver_type);
    void run();

  private:
    void setup_dofs();
    void assemble_system();
    void assemble_multigrid();
    void solve();
    void compute_errors();
    void output_results(const unsigned int refinement_cycle) const;

    const unsigned int pressure_degree;
    const SolverType   solver_type;

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

    AffineConstraints<double> constraints;

    BlockSparsityPattern      sparsity_pattern;
    BlockSparseMatrix<double> system_matrix;
    SparseMatrix<double>      pressure_mass_matrix;

    BlockVector<double> solution;
    BlockVector<double> system_rhs;

    MGLevelObject<SparsityPattern>      mg_sparsity_patterns;
    MGLevelObject<SparseMatrix<double>> mg_matrices;
    MGLevelObject<SparseMatrix<double>> mg_interface_matrices;
    MGConstrainedDoFs                   mg_constrained_dofs;

    TimerOutput computing_timer;
  };

  template <int dim>
  StokesProblem<dim>::StokesProblem(const unsigned int pressure_degree,
                                    const SolverType   solver_type)

    : pressure_degree(pressure_degree)
    , solver_type(solver_type)
    , triangulation(Triangulation<dim>::maximum_smoothing)
    ,

// 仅为速度的有限元。

    velocity_fe(FE_Q<dim>(pressure_degree + 1), dim)
    ,

// 整个系统的有限元。

    fe(velocity_fe, 1, FE_Q<dim>(pressure_degree), 1)
    , dof_handler(triangulation)
    , velocity_dof_handler(triangulation)
    , computing_timer(std::cout, TimerOutput::never, TimerOutput::wall_times)
  {}

//  @sect4{StokesProblem::setup_dofs}

// 这个函数设置了DoFHandler、矩阵、向量和Multigrid结构（如果需要）。

  template <int dim>
  void StokesProblem<dim>::setup_dofs()
  {
    TimerOutput::Scope scope(computing_timer, "Setup");

    system_matrix.clear();
    pressure_mass_matrix.clear();

// 主DoFHandler只需要活动的DoF，所以我们不在这里调用distribution_mg_dofs()

    dof_handler.distribute_dofs(fe);

// 这个块结构将dim速度分量与压力分量（用于重新排序）分开。注意，我们有2个而不是像 step-22 中的dim+1块，因为我们的FESystem是嵌套的，dim速度分量作为一个块出现。

    std::vector<unsigned int> block_component(2);
    block_component[0] = 0;
    block_component[1] = 1;

// 速度从组件0开始。

    const FEValuesExtractors::Vector velocities(0);

//如果我们应用重新排序来减少填充，
//ILU的表现会更好。对于其他求解器来说，这样做并没有什么好处。

    if (solver_type == SolverType::FGMRES_ILU)
      {
        TimerOutput::Scope ilu_specific(computing_timer, "(ILU specific)");
        DoFRenumbering::Cuthill_McKee(dof_handler);
      }

// 这确保了所有的速度DoFs在压力未知数之前被列举出来。这允许我们使用块来处理向量和矩阵，并允许我们为dof_handler和velocity_dof_handler获得相同的DoF编号。

    DoFRenumbering::block_wise(dof_handler);

    if (solver_type == SolverType::FGMRES_GMG)
      {
        TimerOutput::Scope multigrid_specific(computing_timer,
                                              "(Multigrid specific)");
        TimerOutput::Scope setup_multigrid(computing_timer,
                                           "Setup - Multigrid");

// 这将在一个单独的DoFHandler中分配速度空间的主动道夫和多网格道夫，如介绍中所述。

        velocity_dof_handler.distribute_dofs(velocity_fe);
        velocity_dof_handler.distribute_mg_dofs();

// 下面的代码块初始化了MGConstrainedDofs（使用速度的边界条件），以及每个层次的稀疏模式和矩阵。MGLevelObject<T>的resize()函数将破坏所有现有的包含对象。

        std::set<types::boundary_id> zero_boundary_ids;
        zero_boundary_ids.insert(0);

        mg_constrained_dofs.clear();
        mg_constrained_dofs.initialize(velocity_dof_handler);
        mg_constrained_dofs.make_zero_boundary_constraints(velocity_dof_handler,
                                                           zero_boundary_ids);
        const unsigned int n_levels = triangulation.n_levels();

        mg_interface_matrices.resize(0, n_levels - 1);
        mg_matrices.resize(0, n_levels - 1);
        mg_sparsity_patterns.resize(0, n_levels - 1);

        for (unsigned int level = 0; level < n_levels; ++level)
          {
            DynamicSparsityPattern csp(velocity_dof_handler.n_dofs(level),
                                       velocity_dof_handler.n_dofs(level));
            MGTools::make_sparsity_pattern(velocity_dof_handler, csp, level);
            mg_sparsity_patterns[level].copy_from(csp);

            mg_matrices[level].reinit(mg_sparsity_patterns[level]);
            mg_interface_matrices[level].reinit(mg_sparsity_patterns[level]);
          }
      }

    const std::vector<types::global_dof_index> dofs_per_block =
      DoFTools::count_dofs_per_fe_block(dof_handler, block_component);
    const unsigned int n_u = dofs_per_block[0];
    const unsigned int n_p = dofs_per_block[1];

    {
      constraints.clear();

// 下面利用分量掩码对速度的边界值进行插值，这在矢量值dealii  step-20 教程中进一步说明。

      DoFTools::make_hanging_node_constraints(dof_handler, constraints);
      VectorTools::interpolate_boundary_values(dof_handler,
                                               0,
                                               Solution<dim>(),
                                               constraints,
                                               fe.component_mask(velocities));

// 正如在介绍中所讨论的，我们需要固定压力变量的一个自由度以确保问题的可解性。在这里，我们将第一个压力自由度标记为受限自由度，该自由度的索引为n_u。

      if (solver_type == SolverType::UMFPACK)
        constraints.add_line(n_u);

      constraints.close();
    }

    std::cout << "\tNumber of active cells: " << triangulation.n_active_cells()
              << std::endl
              << "\tNumber of degrees of freedom: " << dof_handler.n_dofs()
              << " (" << n_u << '+' << n_p << ')' << std::endl;

    {
      BlockDynamicSparsityPattern csp(dofs_per_block, dofs_per_block);
      DoFTools::make_sparsity_pattern(dof_handler, csp, constraints, false);
      sparsity_pattern.copy_from(csp);
    }
    system_matrix.reinit(sparsity_pattern);

    solution.reinit(dofs_per_block);
    system_rhs.reinit(dofs_per_block);
  }
// @sect4{StokesProblem::assemble_system}

// 在这个函数中，系统矩阵被组装起来。我们在(1,1)块中组装压力质量矩阵（如果需要），并在此函数结束时将其移出此位置。

  template <int dim>
  void StokesProblem<dim>::assemble_system()
  {
    TimerOutput::Scope assemble(computing_timer, "Assemble");
    system_matrix = 0;
    system_rhs    = 0;

// 如果为真，我们将在(1,1)块中装配压力质量矩阵。

    const bool assemble_pressure_mass_matrix =
      (solver_type == SolverType::UMFPACK) ? false : true;

    QGauss<dim> quadrature_formula(pressure_degree + 2);

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

    const unsigned int dofs_per_cell = fe.n_dofs_per_cell();

    const unsigned int n_q_points = quadrature_formula.size();

    FullMatrix<double> local_matrix(dofs_per_cell, dofs_per_cell);
    Vector<double>     local_rhs(dofs_per_cell);

    std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

    const RightHandSide<dim>    right_hand_side;
    std::vector<Vector<double>> rhs_values(n_q_points, Vector<double>(dim + 1));

    const FEValuesExtractors::Vector velocities(0);
    const FEValuesExtractors::Scalar pressure(dim);

    std::vector<SymmetricTensor<2, dim>> symgrad_phi_u(dofs_per_cell);
    std::vector<double>                  div_phi_u(dofs_per_cell);
    std::vector<double>                  phi_p(dofs_per_cell);

    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        fe_values.reinit(cell);
        local_matrix = 0;
        local_rhs    = 0;

        right_hand_side.vector_value_list(fe_values.get_quadrature_points(),
                                          rhs_values);

        for (unsigned int q = 0; q < n_q_points; ++q)
          {
            for (unsigned int k = 0; k < dofs_per_cell; ++k)
              {
                symgrad_phi_u[k] =
                  fe_values[velocities].symmetric_gradient(k, q);
                div_phi_u[k] = fe_values[velocities].divergence(k, q);
                phi_p[k]     = fe_values[pressure].value(k, q);
              }

            for (unsigned int i = 0; i < dofs_per_cell; ++i)
              {
                for (unsigned int j = 0; j <= i; ++j)
                  {
                    local_matrix(i, j) +=
                      (2 * (symgrad_phi_u[i] * symgrad_phi_u[j]) -
                       div_phi_u[i] * phi_p[j] - phi_p[i] * div_phi_u[j] +
                       (assemble_pressure_mass_matrix ? phi_p[i] * phi_p[j] :
                                                        0)) *
                      fe_values.JxW(q);
                  }

                const unsigned int component_i =
                  fe.system_to_component_index(i).first;
                local_rhs(i) += fe_values.shape_value(i, q) *
                                rhs_values[q](component_i) * fe_values.JxW(q);
              }
          }

        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          for (unsigned int j = i + 1; j < dofs_per_cell; ++j)
            local_matrix(i, j) = local_matrix(j, i);

        cell->get_dof_indices(local_dof_indices);
        constraints.distribute_local_to_global(local_matrix,
                                               local_rhs,
                                               local_dof_indices,
                                               system_matrix,
                                               system_rhs);
      }

    if (solver_type != SolverType::UMFPACK)
      {
        pressure_mass_matrix.reinit(sparsity_pattern.block(1, 1));
        pressure_mass_matrix.copy_from(system_matrix.block(1, 1));
        system_matrix.block(1, 1) = 0;
      }
  }
// @sect4{StokesProblem::assemble_multigrid}

// 在这里，与 step-16 中一样，我们有一个函数，用于组装多网格预处理程序所需的水平矩阵和界面矩阵。

  template <int dim>
  void StokesProblem<dim>::assemble_multigrid()
  {
    TimerOutput::Scope multigrid_specific(computing_timer,
                                          "(Multigrid specific)");
    TimerOutput::Scope assemble_multigrid(computing_timer,
                                          "Assemble Multigrid");

    mg_matrices = 0.;

    QGauss<dim> quadrature_formula(pressure_degree + 2);

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

    const unsigned int dofs_per_cell = velocity_fe.n_dofs_per_cell();
    const unsigned int n_q_points    = quadrature_formula.size();

    FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);

    std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

    const FEValuesExtractors::Vector velocities(0);

    std::vector<SymmetricTensor<2, dim>> symgrad_phi_u(dofs_per_cell);

    std::vector<AffineConstraints<double>> boundary_constraints(
      triangulation.n_levels());
    std::vector<AffineConstraints<double>> boundary_interface_constraints(
      triangulation.n_levels());
    for (unsigned int level = 0; level < triangulation.n_levels(); ++level)
      {
        boundary_constraints[level].add_lines(
          mg_constrained_dofs.get_refinement_edge_indices(level));
        boundary_constraints[level].add_lines(
          mg_constrained_dofs.get_boundary_indices(level));
        boundary_constraints[level].close();

        IndexSet idx = mg_constrained_dofs.get_refinement_edge_indices(level) &
                       mg_constrained_dofs.get_boundary_indices(level);

        boundary_interface_constraints[level].add_lines(idx);
        boundary_interface_constraints[level].close();
      }

// 这个迭代器会覆盖所有的单元格（不仅仅是活动的）。

    for (const auto &cell : velocity_dof_handler.cell_iterators())
      {
        fe_values.reinit(cell);
        cell_matrix = 0;

        for (unsigned int q = 0; q < n_q_points; ++q)
          {
            for (unsigned int k = 0; k < dofs_per_cell; ++k)
              symgrad_phi_u[k] = fe_values[velocities].symmetric_gradient(k, q);

            for (unsigned int i = 0; i < dofs_per_cell; ++i)
              for (unsigned int j = 0; j <= i; ++j)
                {
                  cell_matrix(i, j) +=
                    (symgrad_phi_u[i] * symgrad_phi_u[j]) * fe_values.JxW(q);
                }
          }

        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          for (unsigned int j = i + 1; j < dofs_per_cell; ++j)
            cell_matrix(i, j) = cell_matrix(j, i);

        cell->get_mg_dof_indices(local_dof_indices);

        boundary_constraints[cell->level()].distribute_local_to_global(
          cell_matrix, local_dof_indices, mg_matrices[cell->level()]);

        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          for (unsigned int j = 0; j < dofs_per_cell; ++j)
            if (!mg_constrained_dofs.at_refinement_edge(cell->level(),
                                                        local_dof_indices[i]) ||
                mg_constrained_dofs.at_refinement_edge(cell->level(),
                                                       local_dof_indices[j]))
              cell_matrix(i, j) = 0;

        boundary_interface_constraints[cell->level()]
          .distribute_local_to_global(cell_matrix,
                                      local_dof_indices,
                                      mg_interface_matrices[cell->level()]);
      }
  }
// @sect4{StokesProblem::solve}

// 这个函数根据你想使用ILU或GMG作为预处理程序的情况进行不同的设置。 这两种方法共享相同的求解器（FGMRES），但需要初始化不同的预处理器。在这里，我们不仅为整个求解函数计时，还为预处理程序的设置以及求解本身分别计时。

  template <int dim>
  void StokesProblem<dim>::solve()
  {
    TimerOutput::Scope solve(computing_timer, "Solve");
    constraints.set_zero(solution);

    if (solver_type == SolverType::UMFPACK)
      {
        computing_timer.enter_subsection("(UMFPACK specific)");
        computing_timer.enter_subsection("Solve - Initialize");

        SparseDirectUMFPACK A_direct;
        A_direct.initialize(system_matrix);

        computing_timer.leave_subsection();
        computing_timer.leave_subsection();

        {
          TimerOutput::Scope solve_backslash(computing_timer,
                                             "Solve - Backslash");
          A_direct.vmult(solution, system_rhs);
        }

        constraints.distribute(solution);
        return;
      }

// 这里我们必须确保以 "足够好 "的精度求解残差

    SolverControl solver_control(system_matrix.m(),
                                 1e-10 * system_rhs.l2_norm());
    unsigned int  n_iterations_A;
    unsigned int  n_iterations_S;

// 这是用来传递我们是否要在预处理程序中解决A的问题。 我们可以把它改为false，看看是否还能收敛，如果能收敛，那么程序的运行速度是快是慢？

    const bool use_expensive = true;

    SolverFGMRES<BlockVector<double>> solver(solver_control);

    if (solver_type == SolverType::FGMRES_ILU)
      {
        computing_timer.enter_subsection("(ILU specific)");
        computing_timer.enter_subsection("Solve - Set-up Preconditioner");

        std::cout << "   Computing preconditioner..." << std::endl
                  << std::flush;

        SparseILU<double> A_preconditioner;
        A_preconditioner.initialize(system_matrix.block(0, 0));

        SparseILU<double> S_preconditioner;
        S_preconditioner.initialize(pressure_mass_matrix);

        const BlockSchurPreconditioner<SparseILU<double>, SparseILU<double>>
          preconditioner(system_matrix,
                         pressure_mass_matrix,
                         A_preconditioner,
                         S_preconditioner,
                         use_expensive);

        computing_timer.leave_subsection();
        computing_timer.leave_subsection();

        {
          TimerOutput::Scope solve_fmgres(computing_timer, "Solve - FGMRES");

          solver.solve(system_matrix, solution, system_rhs, preconditioner);
          n_iterations_A = preconditioner.n_iterations_A;
          n_iterations_S = preconditioner.n_iterations_S;
        }
      }
    else
      {
        computing_timer.enter_subsection("(Multigrid specific)");
        computing_timer.enter_subsection("Solve - Set-up Preconditioner");

// 在各级之间转移运算符

        MGTransferPrebuilt<Vector<double>> mg_transfer(mg_constrained_dofs);
        mg_transfer.build(velocity_dof_handler);

// 设置粗略的网格解算器

        FullMatrix<double> coarse_matrix;
        coarse_matrix.copy_from(mg_matrices[0]);
        MGCoarseGridHouseholder<double, Vector<double>> coarse_grid_solver;
        coarse_grid_solver.initialize(coarse_matrix);

        using Smoother = PreconditionSOR<SparseMatrix<double>>;
        mg::SmootherRelaxation<Smoother, Vector<double>> mg_smoother;
        mg_smoother.initialize(mg_matrices);
        mg_smoother.set_steps(2);

// Multigrid作为CG的预处理程序时，需要是一个对称的运算器，所以平滑器必须是对称的

        mg_smoother.set_symmetric(true);

        mg::Matrix<Vector<double>> mg_matrix(mg_matrices);
        mg::Matrix<Vector<double>> mg_interface_up(mg_interface_matrices);
        mg::Matrix<Vector<double>> mg_interface_down(mg_interface_matrices);

// 现在，我们准备设置V型循环算子和多级预处理程序。

        Multigrid<Vector<double>> mg(
          mg_matrix, coarse_grid_solver, mg_transfer, mg_smoother, mg_smoother);
        mg.set_edge_matrices(mg_interface_down, mg_interface_up);

        PreconditionMG<dim, Vector<double>, MGTransferPrebuilt<Vector<double>>>
          A_Multigrid(velocity_dof_handler, mg, mg_transfer);

        SparseILU<double> S_preconditioner;
        S_preconditioner.initialize(pressure_mass_matrix,
                                    SparseILU<double>::AdditionalData());

        const BlockSchurPreconditioner<
          PreconditionMG<dim,
                         Vector<double>,
                         MGTransferPrebuilt<Vector<double>>>,
          SparseILU<double>>
          preconditioner(system_matrix,
                         pressure_mass_matrix,
                         A_Multigrid,
                         S_preconditioner,
                         use_expensive);

        computing_timer.leave_subsection();
        computing_timer.leave_subsection();

        {
          TimerOutput::Scope solve_fmgres(computing_timer, "Solve - FGMRES");
          solver.solve(system_matrix, solution, system_rhs, preconditioner);
          n_iterations_A = preconditioner.n_iterations_A;
          n_iterations_S = preconditioner.n_iterations_S;
        }
      }

    constraints.distribute(solution);

    std::cout
      << std::endl
      << "\tNumber of FGMRES iterations: " << solver_control.last_step()
      << std::endl
      << "\tTotal number of iterations used for approximation of A inverse: "
      << n_iterations_A << std::endl
      << "\tTotal number of iterations used for approximation of S inverse: "
      << n_iterations_S << std::endl
      << std::endl;
  }
// @sect4{StokesProblem::process_solution}

// 这个函数计算出解决方案的L2和H1误差。为此，我们需要确保压力的平均值为零。

  template <int dim>
  void StokesProblem<dim>::compute_errors()
  {

// 计算平均压力 $\frac{1}{\Omega} \int_{\Omega} p(x) dx $ ，然后从每个压力系数中减去它。这将产生一个平均值为零的压力。这里我们利用了压力是分量 $dim$ 和有限元空间是结点的事实。

    const double mean_pressure = VectorTools::compute_mean_value(
      dof_handler, QGauss<dim>(pressure_degree + 2), solution, dim);
    solution.block(1).add(-mean_pressure);
    std::cout << "   Note: The mean value was adjusted by " << -mean_pressure
              << std::endl;

    const ComponentSelectFunction<dim> pressure_mask(dim, dim + 1);
    const ComponentSelectFunction<dim> velocity_mask(std::make_pair(0, dim),
                                                     dim + 1);

    Vector<float> difference_per_cell(triangulation.n_active_cells());
    VectorTools::integrate_difference(dof_handler,
                                      solution,
                                      Solution<dim>(),
                                      difference_per_cell,
                                      QGauss<dim>(pressure_degree + 2),
                                      VectorTools::L2_norm,
                                      &velocity_mask);

    const double Velocity_L2_error =
      VectorTools::compute_global_error(triangulation,
                                        difference_per_cell,
                                        VectorTools::L2_norm);

    VectorTools::integrate_difference(dof_handler,
                                      solution,
                                      Solution<dim>(),
                                      difference_per_cell,
                                      QGauss<dim>(pressure_degree + 2),
                                      VectorTools::L2_norm,
                                      &pressure_mask);

    const double Pressure_L2_error =
      VectorTools::compute_global_error(triangulation,
                                        difference_per_cell,
                                        VectorTools::L2_norm);

    VectorTools::integrate_difference(dof_handler,
                                      solution,
                                      Solution<dim>(),
                                      difference_per_cell,
                                      QGauss<dim>(pressure_degree + 2),
                                      VectorTools::H1_norm,
                                      &velocity_mask);

    const double Velocity_H1_error =
      VectorTools::compute_global_error(triangulation,
                                        difference_per_cell,
                                        VectorTools::H1_norm);

    std::cout << std::endl
              << "   Velocity L2 Error: " << Velocity_L2_error << std::endl
              << "   Pressure L2 Error: " << Pressure_L2_error << std::endl
              << "   Velocity H1 Error: " << Velocity_H1_error << std::endl;
  }
// @sect4{StokesProblem::output_results}

// 这个函数生成图形输出，就像在  step-22  中所做的那样。

  template <int dim>
  void
  StokesProblem<dim>::output_results(const unsigned int refinement_cycle) const
  {
    std::vector<std::string> solution_names(dim, "velocity");
    solution_names.emplace_back("pressure");

    std::vector<DataComponentInterpretation::DataComponentInterpretation>
      data_component_interpretation(
        dim, DataComponentInterpretation::component_is_part_of_vector);
    data_component_interpretation.push_back(
      DataComponentInterpretation::component_is_scalar);

    DataOut<dim> data_out;
    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution,
                             solution_names,
                             DataOut<dim>::type_dof_data,
                             data_component_interpretation);
    data_out.build_patches();

    std::ofstream output(
      "solution-" + Utilities::int_to_string(refinement_cycle, 2) + ".vtk");
    data_out.write_vtk(output);
  }

//  @sect4{StokesProblem::run}

// 斯托克斯类的最后一步是像往常一样，生成初始网格的函数，并按各自的顺序调用其他函数。

  template <int dim>
  void StokesProblem<dim>::run()
  {
    GridGenerator::hyper_cube(triangulation);
    triangulation.refine_global(6 - dim);

    if (solver_type == SolverType::FGMRES_ILU)
      std::cout << "Now running with ILU" << std::endl;
    else if (solver_type == SolverType::FGMRES_GMG)
      std::cout << "Now running with Multigrid" << std::endl;
    else
      std::cout << "Now running with UMFPACK" << std::endl;

    for (unsigned int refinement_cycle = 0; refinement_cycle < 3;
         ++refinement_cycle)
      {
        std::cout << "Refinement cycle " << refinement_cycle << std::endl;

        if (refinement_cycle > 0)
          triangulation.refine_global(1);

        std::cout << "   Set-up..." << std::endl;
        setup_dofs();

        std::cout << "   Assembling..." << std::endl;
        assemble_system();

        if (solver_type == SolverType::FGMRES_GMG)
          {
            std::cout << "   Assembling Multigrid..." << std::endl;

            assemble_multigrid();
          }

        std::cout << "   Solving..." << std::flush;
        solve();

        compute_errors();

        output_results(refinement_cycle);

        Utilities::System::MemoryStats mem;
        Utilities::System::get_memory_stats(mem);
        std::cout << "   VM Peak: " << mem.VmPeak << std::endl;

        computing_timer.print_summary();
        computing_timer.reset();
      }
  }
} // namespace Step56
// @sect3{The main function}
int main()
{
  try
    {
      using namespace Step56;

      const int degree = 1;
      const int dim    = 3;

// SolverType的选项。umfpack fgmres_ilu fgmres_gmg

      StokesProblem<dim> flow_problem(degree, SolverType::FGMRES_GMG);

      flow_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 <complex>
#include <limits>

#include <gtest/gtest.h>

#include <Eigen/Dense>
#include <rsvd/StandardNormalRandom.hpp>

using Eigen::Index;
using Rsvd::Internal::standardNormalRandom;

template <typename T> struct StandardNormalRandom : public ::testing::Test {
  using MatrixType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
  using RealType = typename Eigen::NumTraits<T>::Real;

  const Index numRows = 10;
  const Index numCols = 1;
  const Index numTrials = 1'000;
  const unsigned int prngSeed = 777;
  const RealType tol = 1e-1;
};

using NumericalTypes = ::testing::Types<float, double, std::complex<float>, std::complex<double>>;

TYPED_TEST_CASE(StandardNormalRandom, NumericalTypes, );

TYPED_TEST(StandardNormalRandom, ZeroMean) {
  using MatrixType = typename TestFixture::MatrixType;

  MatrixType acc = MatrixType::Zero(TestFixture::numRows, TestFixture::numCols);

  std::mt19937_64 randomEngine;
  randomEngine.seed(TestFixture::prngSeed);
  for (unsigned int i = 0; i < TestFixture::numTrials; ++i) {
    acc += standardNormalRandom<MatrixType, std::mt19937_64>(TestFixture::numRows,
                                                             TestFixture::numCols, randomEngine);
  }
  acc /= TestFixture::numTrials;

  // Since the vector has 10 independent elements, we want to check for the worst case among them
  ASSERT_LE(acc.cwiseAbs().maxCoeff(), TestFixture::tol);
}

TYPED_TEST(StandardNormalRandom, Covariance) {
  using MatrixType = typename TestFixture::MatrixType;

  MatrixType x = MatrixType::Zero(TestFixture::numRows, TestFixture::numTrials);

  std::mt19937_64 randomEngine;
  randomEngine.seed(TestFixture::prngSeed);
  for (unsigned int i = 0; i < TestFixture::numTrials; ++i) {
    x.col(i) = standardNormalRandom<MatrixType, std::mt19937_64>(
        TestFixture::numRows, TestFixture::numCols, randomEngine);
  }
  MatrixType covariance = x * x.adjoint() / TestFixture::numTrials;

  const typename MatrixType::Scalar unitVariance = 1;
  // Check diagonal entries: They should be almost equal to one (unit variance)
  for (Index i = 0; i < TestFixture::numRows; ++i) {
    ASSERT_LE(std::abs(covariance(i, i) - unitVariance), TestFixture::tol);
  }

  // Remove diagonal entries
  covariance.diagonal().setZero();
  // Check all remaining entries: They should be almost zero (independent distributions)
  ASSERT_LE(covariance.cwiseAbs().maxCoeff(), TestFixture::tol);
}


/*
 * L2L2.cpp
 *
 *  Created on: 13.03.2018
 *      Author: thies
 */

#include <deal.II/numerics/vector_tools.h>
#include <norms/L2L2.h>

using namespace dealii;

namespace wavepi {
namespace norms {

template <int dim>
double L2L2<dim>::absolute_error(const DiscretizedFunction<dim>& u, Function<dim>& v) {
  auto mesh     = u.get_mesh();
  double result = 0;

  // trapezoidal rule in time:
  for (size_t i = 0; i < mesh->length(); i++) {
    Vector<double> cellwise_error;

    v.set_time(mesh->get_time(i));
    VectorTools::integrate_difference(*mesh->get_dof_handler(i), u[i], v, cellwise_error, QGauss<dim>(5),
                                      VectorTools::NormType::L2_norm);

    double nrm =
        VectorTools::compute_global_error(*mesh->get_triangulation(i), cellwise_error, VectorTools::NormType::L2_norm);

    if (i > 0) result += nrm * nrm / 2 * (std::abs(mesh->get_time(i) - mesh->get_time(i - 1)));
    if (i < mesh->length() - 1) result += nrm * nrm / 2 * (std::abs(mesh->get_time(i + 1) - mesh->get_time(i)));
  }

  return std::sqrt(result);
}

template <int dim>
double L2L2<dim>::norm(const DiscretizedFunction<dim>& u) const {
  auto mesh     = u.get_mesh();
  double result = 0;

  // trapezoidal rule in time:
  for (size_t i = 0; i < mesh->length(); i++) {
    double nrm2 = mesh->get_mass_matrix(i)->matrix_norm_square(u[i]);

    if (i > 0) result += nrm2 / 2 * (std::abs(mesh->get_time(i) - mesh->get_time(i - 1)));
    if (i < mesh->length() - 1) result += nrm2 / 2 * (std::abs(mesh->get_time(i + 1) - mesh->get_time(i)));
  }

  // assume that function is linear in time (consistent with crank-nicolson!)
  // and integrate that exactly (Simpson rule)
  // problem when mesh changes in time!
  //   for (size_t i = 0; i < mesh->length(); i++) {
  //      double nrm2 = mesh->get_mass_matrix(i)->matrix_norm_square(function_coefficients[i]);
  //
  //      if (i > 0)
  //         result += nrm2 / 3 * (std::abs(mesh->get_time(i) - mesh->get_time(i - 1)));
  //
  //      if (i < mesh->length() - 1)
  //         result += nrm2 / 3 * (std::abs(mesh->get_time(i+1) - mesh->get_time(i)));
  //   }
  //
  //   for (size_t i = 0; i < mesh->length() - 1; i++) {
  //      double tmp = mesh->get_mass_matrix(i)->matrix_scalar_product(function_coefficients[i],
  //            function_coefficients[i + 1]);
  //
  //      result += tmp / 3 * (std::abs(mesh->get_time(i+1) - mesh->get_time(i)));
  //   }

  return std::sqrt(result);
}

template <int dim>
double L2L2<dim>::dot(const DiscretizedFunction<dim>& u, const DiscretizedFunction<dim>& v) const {
  auto mesh     = u.get_mesh();
  double result = 0.0;

  // trapezoidal rule in time:
  for (size_t i = 0; i < mesh->length(); i++) {
    double doti = mesh->get_mass_matrix(i)->matrix_scalar_product(u[i], v[i]);

    if (i > 0) result += doti / 2 * (std::abs(mesh->get_time(i) - mesh->get_time(i - 1)));
    if (i < mesh->length() - 1) result += doti / 2 * (std::abs(mesh->get_time(i + 1) - mesh->get_time(i)));
  }

  // assume that both functions are linear in time (consistent with crank-nicolson!)
  // and integrate that exactly (Simpson rule)
  // problem when mesh changes in time!
  //   for (size_t i = 0; i < mesh->length(); i++) {
  //      Assert(function_coefficients[i].size() == V.function_coefficients[i].size(),
  //            ExcDimensionMismatch (function_coefficients[i].size() , V.function_coefficients[i].size()));
  //
  //      double doti = mesh->get_mass_matrix(i)->matrix_scalar_product(function_coefficients[i],
  //            V.function_coefficients[i]);
  //
  //      if (i > 0)
  //         result += doti / 3 * (std::abs(mesh->get_time(i) - mesh->get_time(i - 1)));
  //
  //      if (i < mesh->length() - 1)
  //         result += doti / 3 * (std::abs(mesh->get_time(i+1) - mesh->get_time(i)));
  //   }
  //
  //   for (size_t i = 0; i < mesh->length() - 1; i++) {
  //      Assert(function_coefficients[i].size() == V.function_coefficients[i+1].size(),
  //            ExcDimensionMismatch (function_coefficients[i].size() , V.function_coefficients[i+1].size()));
  //      Assert(function_coefficients[i+1].size() == V.function_coefficients[i].size(),
  //             ExcDimensionMismatch (function_coefficients[i+1].size() , V.function_coefficients[i].size()));
  //
  //      double dot1 = mesh->get_mass_matrix(i)->matrix_scalar_product(function_coefficients[i],
  //            V.function_coefficients[i + 1]);
  //      double dot2 = mesh->get_mass_matrix(i + 1)->matrix_scalar_product(function_coefficients[i + 1],
  //            V.function_coefficients[i]);
  //
  //      result += (dot1 + dot2) / 6 * (std::abs(mesh->get_time(i+1) - mesh->get_time(i)));
  //   }

  return result;
}

template <int dim>
void L2L2<dim>::dot_transform(DiscretizedFunction<dim>& u) {
  u.mult_mass();
  dot_solve_mass_and_transform(u);
}

template <int dim>
void L2L2<dim>::dot_transform_inverse(DiscretizedFunction<dim>& u) {
  u.solve_mass();
  dot_mult_mass_and_transform_inverse(u);
}

template <int dim>
void L2L2<dim>::dot_solve_mass_and_transform(DiscretizedFunction<dim>& u) {
  auto mesh = u.get_mesh();

  for (size_t i = 0; i < mesh->length(); i++) {
    double factor = 0.0;

    if (i > 0) factor += std::abs(mesh->get_time(i) - mesh->get_time(i - 1)) / 2.0;
    if (i < mesh->length() - 1) factor += std::abs(mesh->get_time(i + 1) - mesh->get_time(i)) / 2.0;

    u[i] *= factor;
  }
}

template <int dim>
void L2L2<dim>::dot_mult_mass_and_transform_inverse(DiscretizedFunction<dim>& u) {
  auto mesh = u.get_mesh();

  // trapezoidal rule in time:
  for (size_t i = 0; i < mesh->length(); i++) {
    double factor = 0.0;

    if (i > 0) factor += std::abs(mesh->get_time(i) - mesh->get_time(i - 1)) / 2.0;
    if (i < mesh->length() - 1) factor += std::abs(mesh->get_time(i + 1) - mesh->get_time(i)) / 2.0;

    u[i] /= factor;
  }
}

template <int dim>
std::string L2L2<dim>::name() const {
  return "L²([0,T], L²(Ω))";
}

template <int dim>
std::string L2L2<dim>::unique_id() const {
  return "L²([0,T], L²(Ω))";
}

template class L2L2<1>;
template class L2L2<2>;
template class L2L2<3>;

} /* namespace norms */
} /* namespace wavepi */


//  Copyright John Maddock 2007.
//  Copyright Paul A> Bristow 2010
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>
using std::cout; using std::endl;
#include <cerrno> // for ::errno

//[policy_eg_1

#include <boost/math/special_functions/gamma.hpp>
using boost::math::tgamma;

// Define the policy to use:
using namespace boost::math::policies; // may be convenient, or

using boost::math::policies::policy;
// Types of error whose action can be altered by policies:.
using boost::math::policies::evaluation_error;
using boost::math::policies::domain_error;
using boost::math::policies::overflow_error;
using boost::math::policies::domain_error;
using boost::math::policies::pole_error;
// Actions on error (in enum error_policy_type):
using boost::math::policies::errno_on_error;
using boost::math::policies::ignore_error;
using boost::math::policies::throw_on_error;
using boost::math::policies::user_error;

typedef policy<
   domain_error<errno_on_error>,
   pole_error<errno_on_error>,
   overflow_error<errno_on_error>,
   evaluation_error<errno_on_error>
> c_policy;
//
// Now use the policy when calling tgamma:

// http://msdn.microsoft.com/en-us/library/t3ayayh1.aspx
// Microsoft errno declared in STDLIB.H as "extern int errno;"

int main()
{
   errno = 0; // Reset.
   cout << "Result of tgamma(30000) is: "
      << tgamma(30000, c_policy()) << endl; // Too big parameter
   cout << "errno = " << errno << endl; // errno 34 Numerical result out of range.
   cout << "Result of tgamma(-10) is: "
      << boost::math::tgamma(-10, c_policy()) << endl; // Negative parameter.
   cout << "errno = " << errno << endl; // error 33 Numerical argument out of domain.
} // int main()

//]

/* Output

policy_eg_1.cpp
  Generating code
  Finished generating code
  policy_eg_1.vcxproj -> J:\Cpp\MathToolkit\test\Math_test\Release\policy_eg_1.exe
  Result of tgamma(30000) is: 1.#INF
  errno = 34
  Result of tgamma(-10) is: 1.#QNAN
  errno = 33

*/