#pragma once

#include <Eigen/Dense>

#include "aabb.hpp"
#include "euclidean-transform.hpp"
#include "ordered-pair.hpp"
#include "perceive/utils/math.hpp"
#include "quaternion.hpp"
#include "vector-2.hpp"
#include "vector-3.hpp"
#include "vector-4.hpp"

namespace perceive
{
// --------------------------------------------------------------------- Lp-norm

enum class Lp_norm_t : int { L_INF = 0, L1 = 1, L2 = 2, L3 = 3, L4 = 4 };

inline const char* str(Lp_norm_t method)
{
   switch(method) {
   case Lp_norm_t::L_INF: return "L-inf";
   case Lp_norm_t::L1: return "L1";
   case Lp_norm_t::L2: return "L2";
   case Lp_norm_t::L3: return "L3";
   case Lp_norm_t::L4: return "L4";
   }
   return "<error>";
}

// --------------------------------------------------------------------- Vectors

typedef Vector2T<float> Vector2f;
typedef Vector3T<float> Vector3f;
typedef Vector4T<float> Vector4f;
typedef Vector4T<float> Plane4f;
typedef QuaternionT<float> QuaternionF;

typedef Vector2T<real> Vector2;
typedef Vector3T<real> Vector3;
typedef Vector4T<real> Vector4;
typedef Vector4T<real> Plane;
typedef QuaternionT<real> Quaternion;
typedef AABBT<real> AABB;

typedef Vector2T<int> Point2;
typedef Vector3T<int> Point3;
typedef Vector4T<int> Point4;
typedef AABBT<int> AABBi;
typedef OrderedPairT<int> OrderedPair;

typedef EuclideanTransformT<float> EuclideanTransformF;
typedef EuclideanTransformT<real> EuclideanTransform;

using RealRange = std::pair<real, real>;

// #define EIGEN_ALIGN Eigen::DontAlign
#define EIGEN_ALIGN Eigen::AutoAlign

template<typename T> using EigenVector4T = Eigen::Matrix<T, 4, 1, EIGEN_ALIGN>;

using Vector2r = Eigen::Vector2d;
using Vector3r = Eigen::Vector3d;
using Vector4r = Eigen::Vector4d;
using Vector6r = Eigen::Matrix<real, 6, 1, EIGEN_ALIGN>;
using VectorXr = Eigen::Matrix<real, Eigen::Dynamic, 1, EIGEN_ALIGN>;

template<typename T> using Matrix4T = Eigen::Matrix<T, 4, 4, EIGEN_ALIGN>;

using Matrix3r  = Eigen::Matrix<real, 3, 3, EIGEN_ALIGN>;
using Matrix4r  = Eigen::Matrix<real, 4, 4, EIGEN_ALIGN>;
using Matrix34r = Eigen::Matrix<real, 3, 4, EIGEN_ALIGN>;
using MatrixXr  = Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic>;

using Matrix3f  = Eigen::Matrix<float, 3, 3, EIGEN_ALIGN>;
using Matrix4f  = Eigen::Matrix<float, 4, 4, EIGEN_ALIGN>;
using Matrix34f = Eigen::Matrix<float, 3, 4, EIGEN_ALIGN>;
using MatrixXf  = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>;

using MatrixXi = Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic>;

// ----------------------------------------------------------------------- Round

// template<> inline Vector2 round<Vector2>(const Vector2& v) noexcept
// {
//    return v.round();
// }
// template<> inline Vector3 round<Vector3>(const Vector3& v) noexcept
// {
//    return v.round();
// }
// template<> inline Vector4 round<Vector4>(const Vector4& v) noexcept
// {
//    return v.round();
// }

// ------------------------------------------------------------------ Conversion

inline Vector2 to_vec2(const Point2& v) noexcept { return Vector2(v(0), v(1)); }
inline Vector2 to_vec2(const Vector2& v) noexcept
{
   return Vector2(v(0), v(1));
}
inline Vector2 to_vec2(const Vector2r& v) noexcept
{
   return Vector2(v(0), v(1));
}
inline Vector2 to_vec2(const Vector2f& v) noexcept
{
   return Vector2(real(v(0)), real(v(1)));
}
inline Vector2r to_vec2r(const Point2& v) noexcept
{
   return Vector2r(v(0), v(1));
}
inline Vector2r to_vec2r(const Vector2& v) noexcept
{
   return Vector2r(v(0), v(1));
}
inline Vector2r to_vec2r(const Vector2r& v) noexcept
{
   return Vector2r(v(0), v(1));
}
inline Vector2r to_vec2r(const Vector2f& v) noexcept
{
   return Vector2r(v(0), v(1));
}
inline Vector2f to_vec2f(const Point2& v) noexcept
{
   return Vector2f(float(v(0)), float(v(1)));
}
inline Vector2f to_vec2f(const Vector2& v) noexcept
{
   return Vector2f(float(v(0)), float(v(1)));
}
inline Vector2f to_vec2f(const Vector2r& v) noexcept
{
   return Vector2f(float(v(0)), float(v(1)));
}
inline Vector2f to_vec2f(const Vector2f& v) noexcept
{
   return Vector2f(v(0), v(1));
}

inline Vector3 to_vec3(const Point3& v) noexcept
{
   return Vector3(v(0), v(1), v(2));
}
inline Vector3 to_vec3(const Vector3& v) noexcept
{
   return Vector3(v(0), v(1), v(2));
}
inline Vector3 to_vec3(const Vector3r& v) noexcept
{
   return Vector3(v(0), v(1), v(2));
}
inline Vector3 to_vec3(const Vector3f& v) noexcept
{
   return Vector3(real(v(0)), real(v(1)), real(v(2)));
}
inline Vector3r to_vec3r(const Point3& v) noexcept
{
   return Vector3r(v(0), v(1), v(2));
}
inline Vector3r to_vec3r(const Vector3& v) noexcept
{
   return Vector3r(v(0), v(1), v(2));
}
inline Vector3r to_vec3r(const Vector3r& v) noexcept
{
   return Vector3r(v(0), v(1), v(2));
}
inline Vector3r to_vec3r(const Vector3f& v) noexcept
{
   return Vector3r(real(v(0)), real(v(1)), real(v(2)));
}
inline Vector3f to_vec3f(const Point3& v) noexcept
{
   return Vector3f(float(v(0)), float(v(1)), float(v(2)));
}
inline Vector3f to_vec3f(const Vector3& v) noexcept
{
   return Vector3f(float(v(0)), float(v(1)), float(v(2)));
}
inline Vector3f to_vec3f(const Vector3r& v) noexcept
{
   return Vector3f(float(v(0)), float(v(1)), float(v(2)));
}
inline Vector3f to_vec3f(const Vector3f& v) noexcept
{
   return Vector3f(v(0), v(1), v(2));
}

inline Vector4 to_vec4(const Point4& v) noexcept
{
   return Vector4(v(0), v(1), v(2), v(3));
}
inline Vector4 to_vec4(const Vector4& v) noexcept
{
   return Vector4(v(0), v(1), v(2), v(3));
}
inline Vector4 to_vec4(const Vector4r& v) noexcept
{
   return Vector4(v(0), v(1), v(2), v(3));
}
inline Vector4 to_vec4(const Vector4f& v) noexcept
{
   return Vector4(real(v(0)), real(v(1)), real(v(2)), real(v(3)));
}
inline Vector4r to_vec4r(const Point4& v) noexcept
{
   return Vector4r(v(0), v(1), v(2), v(3));
}
inline Vector4r to_vec4r(const Vector4& v) noexcept
{
   return Vector4r(v(0), v(1), v(2), v(3));
}
inline Vector4r to_vec4r(const Vector4r& v) noexcept
{
   return Vector4r(v(0), v(1), v(2), v(3));
}
inline Vector4r to_vec4r(const Vector4f& v) noexcept
{
   return Vector4r(real(v(0)), real(v(1)), real(v(2)), real(v(3)));
}
inline Vector4f to_vec4f(const Point4& v) noexcept
{
   return Vector4f(float(v(0)), float(v(1)), float(v(2)), float(v(3)));
}
inline Vector4f to_vec4f(const Vector4& v) noexcept
{
   return Vector4f(float(v(0)), float(v(1)), float(v(2)), float(v(3)));
}
inline Vector4f to_vec4f(const Vector4r& v) noexcept
{
   return Vector4f(float(v(0)), float(v(1)), float(v(2)), float(v(3)));
}
inline Vector4f to_vec4f(const Vector4f& v) noexcept
{
   return Vector4f(float(v(0)), float(v(1)), float(v(2)), float(v(3)));
}

inline Point2 to_pt2(const std::pair<int, int>& v) noexcept
{
   return Point2(v.first, v.second);
}
inline Point2 to_pt2(const Vector2& v) noexcept
{
   return Point2(int(v.x), int(v.y));
}
inline Point2 to_pt2(const Vector2f& v) noexcept
{
   return Point2(int(v.x), int(v.y));
}
inline Point3 to_pt3(const Vector3& v) noexcept
{
   return Point3(int(v.x), int(v.y), int(v.z));
}
inline Point3 to_pt3(const Vector3f& v) noexcept
{
   return Point3(int(v.x), int(v.y), int(v.z));
}
inline Point4 to_pt4(const Vector4& v) noexcept
{
   return Point4(int(v.x), int(v.y), int(v.z), int(v.w));
}
inline Point4 to_pt4(const Vector4f& v) noexcept
{
   return Point4(int(v.x), int(v.y), int(v.z), int(v.w));
}

inline Quaternion to_quaternion(const QuaternionF& q) noexcept
{
   return Quaternion(real(q.x), real(q.y), real(q.z), real(q.w));
}

inline QuaternionF to_quaternionf(const Quaternion& q) noexcept
{
   return QuaternionF(float(q.x), float(q.y), float(q.z), float(q.w));
}

template<typename T> inline Vector2T<T> clip_to_xy(const Vector3T<T>& X)
{
   return Vector2T<T>(X.x, X.y);
}

// ------------------------------------------------------------------- is-finite

inline bool is_finite(const Vector2& x) noexcept { return x.is_finite(); }
inline bool is_finite(const Vector3& x) noexcept { return x.is_finite(); }
inline bool is_finite(const Vector4& x) noexcept { return x.is_finite(); }
inline bool is_finite(const Vector2f& x) noexcept { return x.is_finite(); }
inline bool is_finite(const Vector3f& x) noexcept { return x.is_finite(); }
inline bool is_finite(const Vector4f& x) noexcept { return x.is_finite(); }
inline bool is_finite(float x) noexcept { return std::isfinite(x); }
inline bool is_finite(double x) noexcept { return std::isfinite(x); }

// --------------------------------------------------------- matrix-mult-helpers
// P3
inline Vector4r mult_homgen(const Matrix4r& M, const Vector4r& X) noexcept
{
   Vector4r Y = M * Vector4r(X(0), X(1), X(2), X(3));
   if(fabs(Y(3)) < 1e-9) Y /= Y(3);
   return Y;
}

inline Vector4 mult_homgen(const Matrix4r& M, const Vector4& X) noexcept
{
   return to_vec4(mult_homgen(M, to_vec4r(X)));
}

inline Vector3r mult_homgen(const Matrix4r& M, const Vector3r& X) noexcept
{
   auto Y = mult_homgen(M, Vector4r(X(0), X(1), X(2), 1.0));
   return Vector3r(Y(0), Y(1), Y(2));
}

inline Vector3 mult_homgen(const Matrix4r& M, const Vector3& X) noexcept
{
   return to_vec3(mult_homgen(M, to_vec3r(X)));
}

// P2
inline Vector3r mult_homgen(const Matrix3r& M, const Vector3r& X) noexcept
{
   Vector3r Y = M * X;
   if(fabs(Y(2)) < 1e-9) Y /= Y(2);
   return Y;
}

inline Vector3 mult_homgen(const Matrix3r& M, const Vector3& X) noexcept
{
   return to_vec3(mult_homgen(M, to_vec3r(X)));
}

inline Vector2r mult_homgen(const Matrix3r& M, const Vector2r& X) noexcept
{
   auto Y = mult_homgen(M, Vector3r(X(0), X(1), 1.0));
   return Vector2r(Y(0), Y(1));
}

inline Vector2 mult_homgen(const Matrix3r& M, const Vector2& X) noexcept
{
   return to_vec2(mult_homgen(M, to_vec2r(X)));
}

// --------------------------------------------------------------- AABB => AABBi

inline AABBi aabb_to_aabbi(const AABB& aabb)
{
   return AABBi(int(floor(aabb.left)),
                int(floor(aabb.top)),
                int(ceil(aabb.right)),
                int(ceil(aabb.bottom)));
}

// -------------------------------------------------------- Matrix3r => Matrix3d

template<typename U, typename V> void matrixU_to_V(const U& S, V& D) noexcept
{
   using T               = typename V::Scalar;
   const unsigned n_rows = unsigned(S.rows());
   const unsigned n_cols = unsigned(S.cols());
   if(D.rows() != S.rows() || D.cols() != S.cols())
      D = V::Zero(S.rows(), S.cols());
   assert(D.rows() == n_rows && D.cols() == n_cols);
   for(unsigned row = 0; row < n_rows; ++row)
      for(unsigned col = 0; col < n_cols; ++col) D(row, col) = T(S(row, col));
}

// static void matrix3r_to_3d(const Matrix3r& S, Matrix3d& D) {matrixU_to_V(S,
// D);} static void matrix3d_to_3r(const Matrix3d& S, Matrix3r& D)
// {matrixU_to_V(S, D);} static void matrixXr_to_Xd(const MatrixXr& S, MatrixXd&
// D) {matrixU_to_V(S, D);} static void matrixXd_to_Xr(const MatrixXd& S,
// MatrixXr& D) {matrixU_to_V(S, D);}

// --------------------------------------------------------------------- Generic

template<typename T> typename T::value_type quadrance(const T& v) noexcept
{
   return v.quadrance();
}
template<typename T> typename T::value_type norm(const T& v) noexcept
{
   return v.norm();
}
template<typename T> typename T::value_type dot(const T& u, const T& v) noexcept
{
   return u.dot(v);
}
template<typename T>
typename T::value_type distance(const T& u, const T& v) noexcept
{
   return u.distance(v);
}

template<typename T>
inline Vector3T<T> cross(const Vector3T<T>& u, const Vector3T<T>& v)
{
   return u.cross(v);
}

inline Matrix3r make_skew_symmetric(const Vector3& t) noexcept
{
   Matrix3r Tx = Matrix3r::Zero();
   Tx(0, 1)    = -t(2);
   Tx(0, 2)    = t(1);
   Tx(1, 2)    = -t(0);
   Tx(1, 0)    = t(2);
   Tx(2, 0)    = -t(1);
   Tx(2, 1)    = t(0);
   return Tx;
}

// --------------------------------------------------------------------- Vector2

inline Vector2 cartesian_to_polar(const Vector2& x) noexcept
{
   Vector2 ret;
   if(fabs(x.quadrance() - real(1.0)) < real(1e-9)) {
      ret.mag()   = real(1.0);
      ret.theta() = acos(x.y >= real(0.0) ? x.x : -x.x);
   } else {
      ret.mag()   = x.norm();
      ret.theta() = acos((x.y >= real(0.0) ? x.x : -x.x) / ret.mag());
   }
   return ret;
}

inline Vector2 angle_to_cartesian(const real theta) noexcept
{
   return Vector2(cos(theta), sin(theta));
}

inline Vector2 polar_to_cartesian(const Vector2& x) noexcept
{
   return angle_to_cartesian(x.theta()) * x.mag();
}

// ------------------------------------------------------------------- Spherical

inline Vector3f
spherical_to_cartesian(float inc, float azi, float dist) noexcept
{
   const float sinx = std::sin(inc);
   const float cosx = std::cos(inc);
   const float siny = std::sin(azi);
   const float cosy = std::cos(azi);
   //
   return Vector3f(sinx * cosy, sinx * siny, cosx) * dist;
}

inline Vector3 spherical_to_cartesian(real inc, real azi, real dist) noexcept
{
   const Vector3::value_type sinx = sin(inc);
   const Vector3::value_type cosx = cos(inc);
   const Vector3::value_type siny = sin(azi);
   const Vector3::value_type cosy = cos(azi);
   //
   return Vector3(sinx * cosy, sinx * siny, cosx) * dist;
}

inline Vector3 spherical_to_cartesian(const Vector3& s) noexcept
{
   return spherical_to_cartesian(s.x, s.y, s.z);
}

inline Vector3f spherical_to_cartesian(const Vector3f& s) noexcept
{
   return spherical_to_cartesian(s.x, s.y, s.z);
}

inline Vector3 cartesian_to_spherical(real x, real y, real z) noexcept
{
   auto r           = sqrt(x * x + y * y + z * z);
   auto inclination = acos(z / r);
   auto azimuth     = atan2(y, x);
   return Vector3(inclination, azimuth, r);
}

inline Vector3 cartesian_to_spherical(const Vector3& n) noexcept
{
   return cartesian_to_spherical(n.x, n.y, n.z);
}

inline Vector3f cartesian_to_spherical(const Vector3f& n) noexcept
{
   return to_vec3f(cartesian_to_spherical(real(n.x), real(n.y), real(n.z)));
}

// ----------------------------------------------------------------- Homogeneous

inline Vector3r normalized_P2(const Vector3r& x) noexcept
{
   Vector3r v = x;
   v /= v(2);
   if((fabs(v(0)) > 1e20) || (fabs(v(1)) > 1e20))
      return x; // tending towards an ideal point
   return v;
}

template<typename T>
inline Vector2T<T> homgen_P2_to_R2(const Vector3T<T>& x) noexcept
{
   if(std::fabs(x.z) < T(1e-20)) {
      T sign = (x.z < T(0.0)) ? -T(1.0) : T(1.0);
      return Vector2T<T>(sign * x.x / T(1e-20), sign * x.y / T(1e-20));
   }
   auto inv = T(1.0) / x.z;
   return Vector2T<T>(x.x * inv, x.y * inv);
}

template<typename T>
inline Vector3T<T> homgen_R2_to_P2(const Vector2T<T>& x) noexcept
{
   return Vector3T<T>(x.x, x.y, T(1.0));
}

template<typename T>
inline Vector2T<T> line_line_isect(const Vector3T<T>& u,
                                   const Vector3T<T>& v) noexcept
{
   return homgen_P2_to_R2(cross(u, v));
}

template<typename T>
inline Vector3T<T> to_homgen_line(const Vector2T<T>& u,
                                  const Vector2T<T>& v) noexcept
{
   Vector3T<T> ret(u.y - v.y, v.x - u.x, u.x * v.y - u.y * v.x);
   ret.normalise_line();
   return ret;
}

template<typename T>
inline Vector3T<T> to_homgen_line(const Vector3T<T>& u,
                                  const Vector3T<T>& v) noexcept
{
   Vector3T<T> ret = cross(u, v);
   ret.normalise_line();
   return ret;
}

inline Vector3r to_homgen_line(const Vector3r& u, const Vector3r& v) noexcept
{
   return to_vec3r(to_homgen_line(to_vec3(u), to_vec3(v)));
}

template<typename T>
inline T dot_line_point(const Vector3T<T>& line,
                        const Vector3T<T>& point) noexcept
{
   return dot(line, point);
}

template<typename T>
inline T dot_line_point(const Vector3T<T>& line,
                        const Vector2T<T>& point) noexcept
{
   return line.x * point.x + line.y * point.y + line.z;
}

// Project 'point' onto 'line'
template<typename T>
inline Vector2T<T> project_point_line(const Vector3T<T>& line,
                                      const Vector2T<T>& point) noexcept
{
   return point - dot_line_point(line, point) * Vector2T<T>(line.x, line.y);
}

inline bool is_orthogonal(const Vector3& a, const Vector3& b) noexcept
{
   return fabs(dot(a, b)) < 1e-9;
}

template<typename T>
inline typename T::value_type uv_cos_theta(const T& u, const T& v)
{
   using V = typename T::value_type;
   return std::clamp<V>(u.normalised().dot(v.normalised()), V(-1.0), V(1.0));
}

// ---------------------------------------------------------- Lifted Coordinates

inline Vector6r lift_xy(real x, real y) noexcept
{
   Vector6r X;
   X(0) = square(x);
   X(1) = x * y;
   X(2) = square(y);
   X(3) = x;
   X(4) = y;
   X(5) = 1.0;
   return X;
}

inline Vector6r lift_xy(const Vector2& x) noexcept { return lift_xy(x.x, x.y); }
inline Vector6r lift_xy(const Vector2r& x) noexcept
{
   return lift_xy(x(0), x(1));
}
inline Vector6r lift_xy(const Vector3& x) noexcept
{
   return lift_xy(homgen_P2_to_R2(x));
}
inline Vector6r lift_xy(const Vector3r& x) noexcept
{
   return lift_xy(homgen_P2_to_R2(to_vec3(x)));
}

// ------------------------------------------------------------------------ Rays

inline Vector3 to_ray(const MatrixXr& A, real x, real y) noexcept
{
   return to_vec3(A * lift_xy(x, y)).normalized();
}

inline Vector3 to_ray(const MatrixXr& A, const Vector2& x) noexcept
{
   return to_ray(A, x.x, x.y);
}
inline Vector3 to_ray(const MatrixXr& A, const Vector2r& x) noexcept
{
   return to_ray(A, x(0), x(1));
}
inline Vector3 to_ray(const MatrixXr& A, const Vector3r& x) noexcept
{
   return to_ray(A, homgen_P2_to_R2(to_vec3(x)));
}

// ---------------------------------------------------------------- 2D Iteration

static constexpr array<std::pair<int, int>, 4> four_connected
    = {{{-1, 0}, {0, -1}, {0, 1}, {1, 0}}};

static constexpr array<std::pair<int, int>, 8> eight_connected
    = {{{-1, 0}, {0, -1}, {0, 1}, {1, 0}, {-1, -1}, {-1, 1}, {1, -1}, {1, 1}}};

static constexpr array<std::pair<int, int>, 20> twenty_connected
    = {{{-2, -1}, {-2, 0}, {-2, 1}, {-1, -2}, {-1, -1}, {-1, 0}, {-1, 1},
        {-1, 2},  {0, -2}, {0, -1}, {0, 1},   {0, 2},   {1, -2}, {1, -1},
        {1, 0},   {1, 1},  {1, 2},  {2, -1},  {2, 0},   {2, 1}}};

static constexpr array<std::pair<int, int>, 21> twentyone_connected
    = {{{-2, -1}, {-2, 0}, {-2, 1}, {-1, -2}, {-1, -1}, {-1, 0}, {-1, 1},
        {-1, 2},  {0, -2}, {0, -1}, {0, 1},   {0, 0},   {0, 2},  {1, -2},
        {1, -1},  {1, 0},  {1, 1},  {1, 2},   {2, -1},  {2, 0},  {2, 1}}};

static constexpr array<std::pair<int, int>, 24> twentyfour_connected
    = {{{-2, -2}, {-2, -1}, {-2, 0}, {-2, 1}, {-2, 2}, {-1, -2},
        {-1, -1}, {-1, 0},  {-1, 1}, {-1, 2}, {0, -2}, {0, -1},
        {0, 1},   {0, 2},   {1, -2}, {1, -1}, {1, 0},  {1, 1},
        {1, 2},   {2, -2},  {2, -1}, {2, 0},  {2, 1},  {2, 2}}};

// -------------------------------------------------------------------- feedback

template<typename T>
inline string vec_feedback(const Vector2T<T>& u, const Vector2T<T>& v) noexcept
{
   return format("|{} - {}| = {}", str(u), str(v), (u - v).norm());
}

template<typename T>
inline string vec_feedback(const Vector3T<T>& u, const Vector3T<T>& v) noexcept
{
   return format("|{} - {}| = {}", str(u), str(v), (u - v).norm());
}

template<typename T>
inline string vec_feedback(const Vector4T<T>& u, const Vector4T<T>& v) noexcept
{
   return format("|{} - {}| = {}", str(u), str(v), (u - v).norm());
}

// ----------------------------------------------------------- Quantize Funciton
std::array<std::pair<Point2, float>, 4> quantize(const Vector2& X) noexcept;

} // namespace perceive

namespace std
{
// OrderedPair
template<> struct hash<perceive::OrderedPair>
{
   size_t operator()(const perceive::OrderedPair& v) const { return v.hash(); }
};

// ---------------------------------------------------------------------- Vector

// Vector2
template<> struct hash<perceive::Vector2>
{
   size_t operator()(const perceive::Vector2& v) const { return v.hash(); }
};

// Vector3
template<> struct hash<perceive::Vector3>
{
   size_t operator()(const perceive::Vector3& v) const { return v.hash(); }
};

// Vector4
template<> struct hash<perceive::Vector4>
{
   size_t operator()(const perceive::Vector4& v) const { return v.hash(); }
};

// ----------------------------------------------------------------------- Point

// Point2
template<> struct hash<perceive::Point2>
{
   size_t operator()(const perceive::Point2& v) const { return v.hash(); }
};

// Point3
template<> struct hash<perceive::Point3>
{
   size_t operator()(const perceive::Point3& v) const { return v.hash(); }
};

// Point4
template<> struct hash<perceive::Point4>
{
   size_t operator()(const perceive::Point4& v) const { return v.hash(); }
};

} // namespace std


// Copyright 2015-2018 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

//[ guide_make_static_histogram

#include <boost/histogram.hpp>
#include <cassert>

int main() {
  using namespace boost::histogram;

  // create a 1d-histogram in default configuration which
  // covers the real line from -1 to 1 in 100 bins
  auto h = make_histogram(axis::regular<>(100, -1.0, 1.0));

  // rank is the number of axes
  assert(h.rank() == 1);
}

//]


// test_differentiate.cpp
// (c) Tivole

#include <boost/test/unit_test.hpp>
#include "../src/numerary.hpp"

namespace numerary
{

    BOOST_AUTO_TEST_SUITE(TestDifferentiate)

    BOOST_AUTO_TEST_CASE(test_differentiate_order_1_h)
    {
        const double h = 0.00001;
        const short int order = 1;
        double x;
        double expected_result;
        double result;

        // Testing sin
        x = M_PI;
        expected_result = -1;
        result = Numerary::differentiate(sin, 1, x, "h", h);
        BOOST_CHECK(fabs(result - expected_result) <= 1.e-4);

        // Testing cos
        x = M_PI_2;
        expected_result = -1;
        result = Numerary::differentiate(cos, 1, x, "h", h);
        BOOST_CHECK(fabs(result - expected_result) <= 1.e-4);

        // Testing exp
        x = 1;
        expected_result = 2.718281828;
        result = Numerary::differentiate(exp, 1, x, "h", h);
        BOOST_CHECK(fabs(result - expected_result) <= 1.e-4);
    }

    BOOST_AUTO_TEST_CASE(test_differentiate_order_1_2h)
    {
        const double h = 0.00001;
        const short int order = 1;
        double x;
        double expected_result;
        double result;

        // Testing sin
        x = M_PI;
        expected_result = -1;
        result = Numerary::differentiate(sin, 1, x, "2h", h);
        BOOST_CHECK(fabs(result - expected_result) <= 1.e-4);

        // Testing cos
        x = M_PI_2;
        expected_result = -1;
        result = Numerary::differentiate(cos, 1, x, "2h", h);
        BOOST_CHECK(fabs(result - expected_result) <= 1.e-4);

        // Testing exp
        x = 1;
        expected_result = 2.718281828;
        result = Numerary::differentiate(exp, 1, x, "2h", h);
        BOOST_CHECK(fabs(result - expected_result) <= 1.e-4);
    }

    BOOST_AUTO_TEST_SUITE_END()
}


//==============================================================================
//         Copyright 2015 J.T.Lapreste
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EULER_FUNCTIONS_SIMD_COMMON_BETALN_HPP_INCLUDED
#define NT2_EULER_FUNCTIONS_SIMD_COMMON_BETALN_HPP_INCLUDED

#include <nt2/euler/functions/betaln.hpp>
#include <nt2/include/functions/simd/gammaln.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/plus.hpp>
#include <nt2/include/functions/simd/all.hpp>
#include <nt2/include/functions/simd/logical_and.hpp>
#include <nt2/include/functions/simd/is_nltz.hpp>
#include <boost/assert.hpp>
#include <boost/simd/operator/functions/details/assert_utils.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( betaln_, tag::cpu_
                            , (A0)
                            , ((generic_<floating_<A0>>))
                              ((generic_<floating_<A0>>))
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL_REPEAT(2)
    {
      BOOST_ASSERT_MSG(boost::simd::assert_all(logical_and(is_nltz(a0),is_nltz(a1))), "inputs must be positive");
      return(gammaln(a0)+gammaln(a1)-gammaln(a0+a1));
    }
  };
} }

#endif


//
// Copyright (c) 2012 Juan Palacios juan.palacios.puyana@gmail.com
// This file is part of minimathlibs.
// Subject to the BSD 2-Clause License
// - see < http://opensource.org/licenses/BSD-2-Clause>
//

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE TestMatrix
#include <boost/test/unit_test.hpp>

#include <cmath>
#include <cstdlib>
#include <iostream>
#include <limits>
#include "minimath/matrix.hpp"
#include "minimath/matrix_ops.hpp"
#include "minimath/numeric_utils.hpp"

namespace
{
// test if all elements of a matrix have a particular value
template <typename M>
bool valueEquality(const M& m,
                   typename M::value_type v,
                   typename M::value_type tol = std::numeric_limits<typename M::value_type>::epsilon()) {

  for (unsigned int r = 0; r < m.rows(); ++r)
  {
    for (unsigned int c = 0; c < m.cols(); ++c)
    {
      if ( std::abs(m(r,c) - v) > tol) {
        std::cout << "\nm(" << r << ", " << c << ") = " << m(r,c) << "\n";
        std::cout << "\n" << m << "\n";
        return false;
      }
    }
  }
  return true;
}

// test if two elements are within epsilon of each other
template <typename T>
bool valueEquality(const T& lhs, const T& rhs)
{
  return std::abs(rhs-lhs) < std::numeric_limits<T>::epsilon();
}

// test if a matrix is an identity matrix
template <typename M>
bool isIdentity(const M& m,
                typename M::value_type tol = std::numeric_limits<typename M::value_type>::epsilon()) {

  if (m.rows() != m.cols()) return false;
  for (unsigned int r = 0; r < m.rows(); ++r)
  {
    for (unsigned int c = 0; c < m.cols(); ++c)
    {
      if (r==c)
      {
        if ( std::abs(m(r,c) - 1) > tol) return false;
      } else {
        if ( std::abs(m(r,c)) > tol) {
          std::cout << "\nm(" << r << ", " << c << ") = " << m(r,c) << "\n";
          std::cout << "\n" << m << "\n";
          return false;
        }
      }
    }
  }
  return true;
}

struct setup
{
    setup() { std::srand(42); }
};

} // anonymous namespace

typedef minimath::matrix<double, 2> M2x2;
typedef minimath::matrix<double, 3> M3x3;
typedef minimath::matrix<double, 4> M4x4;
typedef minimath::matrix<double, 5> M5x5;

typedef minimath::matrix<double, 2,1> M2x1;
typedef minimath::matrix<double, 3,2> M3x2;
typedef minimath::matrix<double, 4,3> M4x3;
typedef minimath::matrix<double, 5,4> M5x4;

typedef minimath::matrix<double, 3,4> M3x4;
typedef minimath::matrix<double, 4,5> M4x5;

BOOST_FIXTURE_TEST_SUITE(TestMatrix, setup)

BOOST_AUTO_TEST_CASE(testDefaultConstruction)
{
  M2x2 a;
  M2x2 b;
  M2x2 c;
  BOOST_CHECK(valueEquality(a, 0.));
  BOOST_CHECK(valueEquality(b, 0.));
  BOOST_CHECK(valueEquality(c, 0.));
  BOOST_CHECK(2==a.rows());
  BOOST_CHECK(2==a.cols());
  BOOST_CHECK(4==a.size());
  BOOST_CHECK(a==b);
  BOOST_CHECK(a==c);
  BOOST_CHECK(b==c);

  M5x4 d, e, f;
  BOOST_CHECK(valueEquality(d, 0));
  BOOST_CHECK(valueEquality(e, 0));
  BOOST_CHECK(valueEquality(f, 0));
  BOOST_CHECK(5==e.rows());
  BOOST_CHECK(4==e.cols());
  BOOST_CHECK(20==e.size());
  BOOST_CHECK(d==e);
  BOOST_CHECK(d==f);
  BOOST_CHECK(e==f);

}

BOOST_AUTO_TEST_CASE(testZeroMatrixConstruction)
{
  M4x3 m = minimath::zero_matrix();
  BOOST_CHECK(valueEquality(m, 0));
}

BOOST_AUTO_TEST_CASE(testZeroMatrixAssignment)
{
  M4x3 m;
  m = minimath::zero_matrix();
  BOOST_CHECK(valueEquality(m, 0));
}

BOOST_AUTO_TEST_CASE(testIdentityMatrixConstruction)
{
  M4x4 m4 = minimath::identity_matrix();
  BOOST_CHECK(isIdentity(m4));
  M5x5 m5 = minimath::identity_matrix();
  BOOST_CHECK(isIdentity(m5));

}

BOOST_AUTO_TEST_CASE(testIdentityMatrixAssignment)
{
  M4x4 m;
  m = minimath::identity_matrix();
  BOOST_CHECK(isIdentity(m));
}

BOOST_AUTO_TEST_CASE(testScalarConstruction)
{
  for (int  i = -20; i < 21; ++i) {
    M5x5 m(i);
    BOOST_CHECK(valueEquality(m, i));
  }
}

BOOST_AUTO_TEST_CASE(testCopyConstruction)
{
  M4x3 m;
  for (unsigned int i = 0; i< m.size(); ++i) {
    m[i] = i;
  }
  M4x3 m2(m);
  BOOST_CHECK(m2==m);
}

BOOST_AUTO_TEST_CASE(testAssignment)
{
  M4x3 m;
  for (unsigned int i = 0; i< m.size(); ++i) {
    m[i] = i;
  }
  M4x3 m2;
  m2 = m;
  BOOST_CHECK(m2==m);

}

BOOST_AUTO_TEST_CASE(testEquality)
{
  M4x3 m1, m2;
  for (unsigned int i = 0; i< m1.size(); ++i) {
    m1[i] = i;
    m2[i] = i;
  }
  BOOST_CHECK(m2==m1);

}

BOOST_AUTO_TEST_CASE(testInequality)
{
  M4x3 m;
  for (unsigned int i = 0; i< m.size(); ++i) {
    m[i] = i;
  }
  M4x3 m2;
  BOOST_CHECK(m2!=m);
}

BOOST_AUTO_TEST_CASE(testPlusEqualsScalar)
{
  M4x3 m;
  for (unsigned int i = 0; i< m.size(); ++i) {
    m[i] = i;
  }
  m += 100;
  for (unsigned int i = 0; i< m.size(); ++i) {
    BOOST_CHECK(valueEquality(m[i], M4x3::value_type(i) + 100));
  }
}

BOOST_AUTO_TEST_CASE(testMinusEqualsScalar)
{
  M4x3 m;
  for (unsigned int i = 0; i< m.size(); ++i) {
    m[i] = i;
  }
  m -= 100;
  for (unsigned int i = 0; i< m.size(); ++i) {
    BOOST_CHECK(valueEquality(m[i], M4x3::value_type(i) - 100.));
  }
}

BOOST_AUTO_TEST_CASE(testPlusEquals)
{
  M3x3 m1;
  for (unsigned int i = 0; i< m1.size(); ++i) {
    m1[i] = i;
  }
  M3x3 m2(10);
  m1 += m2;
  for (unsigned int i = 0; i< m1.size(); ++i) {
    BOOST_CHECK(valueEquality(m1[i], M3x3::value_type(i) + 10));
  }

}

BOOST_AUTO_TEST_CASE(testMinusEquals)
{
  M3x3 m1;
  for (unsigned int i = 0; i< m1.size(); ++i) {
    m1[i] = 10*i;
  }
  M3x3 m2(10);
  m1 -= m2;
  for (unsigned int i = 0; i< m1.size(); ++i) {
    BOOST_CHECK(valueEquality(m1[i], M3x3::value_type(10)*(int(i) -1) ));
  }
}

BOOST_AUTO_TEST_CASE(testTimesEqualsScalar)
{
  M4x3 m;
  for (unsigned int i = 0; i< m.size(); ++i) {
    m[i] = i;
  }
  m *= 100;
  for (unsigned int i = 0; i< m.size(); ++i) {
    BOOST_CHECK(valueEquality(m[i], M4x3::value_type(i) * 100));
  }
}

BOOST_AUTO_TEST_CASE(testDivideEqualsScalar)
{
  M4x3 m;
  for (unsigned int i = 0; i< m.size(); ++i) {
    m[i] = i*1000;
  }
  m /= 100;
  for (unsigned int i = 0; i< m.size(); ++i) {
    BOOST_CHECK(valueEquality(m[i], M4x4::value_type(i) * 10));
  }
}

BOOST_AUTO_TEST_CASE(testMatrixPlusScalar)
{
  M4x3 m(11);
  M4x3 m2 = m + 100;
  for (unsigned int i = 0; i< m2.size(); ++i) {
    BOOST_CHECK(valueEquality(m2, 111));
  }
}

BOOST_AUTO_TEST_CASE(testMatrixMinusScalar)
{
  M4x3 m(111);
  M4x3 m2 = m - 101;
  for (unsigned int i = 0; i< m2.size(); ++i) {
    BOOST_CHECK(valueEquality(m2, 10));
  }
}

BOOST_AUTO_TEST_CASE(testScalarPlusMatrix)
{
  M4x3 m(11);
  M4x3 m2 = 100 + m;
  for (unsigned int i = 0; i< m2.size(); ++i) {
    BOOST_CHECK(valueEquality(m2, 111));
  }
}

BOOST_AUTO_TEST_CASE(testScalarMinusMatrix)
{
  M4x3 m(101);
  M4x3 m2 = 111 - m;
  for (unsigned int i = 0; i< m2.size(); ++i) {
    BOOST_CHECK(valueEquality(m2, 10));
  }
}

BOOST_AUTO_TEST_CASE(testMatrixTimesScalar)
{
  M4x3 m(11);
  M4x3 m2 = m * 100;
  for (unsigned int i = 0; i< m2.size(); ++i) {
    BOOST_CHECK(valueEquality(m2, 1100));
  }
}

BOOST_AUTO_TEST_CASE(testTranspose)
{
  M4x3 m;
  for (unsigned int i = 0; i< m.size(); ++i) {
    m[i] = i;
  }
  M3x4 mT = m.transpose();
  for (unsigned int r = 0; r < m.rows(); ++r)
  {
    for (unsigned int c = 0; c < m.cols(); ++c)
    {
      BOOST_CHECK(valueEquality(m(r,c), mT(c,r)));
    }
  }
}

BOOST_AUTO_TEST_CASE(testLeftInverse)
{
  for (unsigned int attempt = 0; attempt <5; ++attempt)
  {
    M4x3 m;
    for (unsigned int i = 0; i< m.size(); ++i) {
      m[i] = std::rand()%m.size();
    }
    bool success = true;
    M3x4 mInv = minimath::left_inverse(m, success);
    BOOST_CHECK(success);
    BOOST_CHECK(minimath::equal(mInv*m, M3x3(minimath::identity_matrix()), 128));
  }
}

BOOST_AUTO_TEST_CASE(testInverse)
{
  for (unsigned int attempt = 0; attempt <5; ++attempt)
  {
    M3x3 m;
    for (unsigned int i = 0; i< m.size(); ++i) {
      m[i] = std::rand()%m.size();
    }
    bool success = true;
    M3x3 mInv = m.inverse(success);
    BOOST_CHECK(success);
    BOOST_CHECK(minimath::equal(mInv*m, M3x3(minimath::identity_matrix()), 128));
  }
}

BOOST_AUTO_TEST_CASE(testInvert)
{
  for (unsigned int attempt = 0; attempt <5; ++attempt)
  {
    M3x3 m;
    for (unsigned int i = 0; i< m.size(); ++i) {
      m[i] = std::rand()%m.size();
    }
    bool success = true;
    M3x3 mInv = m;
    mInv.invert(success);
    BOOST_CHECK(success);
    BOOST_CHECK(minimath::equal(mInv*m, M3x3(minimath::identity_matrix()), 16));
  }
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/lexical_cast.hpp>
#include <iostream>
#include <vector>

class hamiltonian {
public:
  // z[0]: position, z[1]: momentum
  double energy(double t, std::vector<double> const& z) const {
    return potential_energy(t, z) + kinetic_energy(t, z);
  }
  double potential_energy(double /* t */, std::vector<double> const& z) const {
    //return -std::cos(2*M_PI*z[0]);
    return -std::cos(z[0]);
  }
  double kinetic_energy(double /* t */, std::vector<double> const& z) const {
    return 0.5 * z[1] * z[1];
  }
  // "force" calculation
  void operator()(double /* t */, std::vector<double> const& z, std::vector<double>& force) const {
    force[0] = z[1];
    //force[1] = -2*M_PI*std::sin(2*M_PI*z[0]);
    force[1] = -std::sin(z[0]);
  }
};

int main(int argc, char **argv) {
  double t_init = 0;
  double t_final = 10;
  double dt = 0.001;
  if (argc >= 2) t_final = boost::lexical_cast<double>(argv[1]);
  if (argc >= 3) dt = boost::lexical_cast<double>(argv[2]);
  std::cout << "# " << integrator_t::name() << std::endl
            << "# t_init  = " << t_init << std::endl
            << "# t_final = " << t_final << std::endl
            << "# dt      = " << dt << std::endl;

  std::vector<double> z(2);
  z[0] = M_PI*0.5;
  z[1] = 0;

  hamiltonian ham;

  integrator_t integrator(2);
  for (double t = t_init; t <= t_final; t += dt) {
    std::cout <<  t << ' ' << z[0] << ' ' << z[1] << ' ' << ham.energy(t,z) << ' ' << ham.kinetic_energy(t,z) << ' ' << ham.potential_energy(t,z) << std::endl;
    integrator.step(t, dt, z, ham);
  }
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2018 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE at
 * the top level of the deal.II distribution.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Wolfgang Bangerth, Colorado State University
 *         Yong-Yong Cai, Beijing Computational Science Research Center
 */

// @sect3{Include files}  程序以通常的包含文件开始，所有这些文件你现在应该都见过了。

#include <deal.II/base/logstream.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/block_sparse_matrix.h>
#include <deal.II/lac/block_vector.h>
#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/sparse_direct.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/error_estimator.h>
#include <deal.II/numerics/matrix_tools.h>

#include <fstream>
#include <iostream>

// 然后按照惯例将这个程序的所有内容放入一个命名空间，并将deal.II命名空间导入到我们将要工作的命名空间中。

namespace Step58
{
  using namespace dealii;
// @sect3{The NonlinearSchroedingerEquation class}

// 然后是主类。它看起来非常像  step-4  或  step-6  中的相应类，唯一的例外是，矩阵和向量以及其他所有与线性系统相关的元素现在都存储为  `std::complex<double>`  类型，而不仅仅是 `double`。

  template <int dim>
  class NonlinearSchroedingerEquation
  {
  public:
    NonlinearSchroedingerEquation();
    void run();

  private:
    void setup_system();
    void assemble_matrices();
    void do_half_phase_step();
    void do_full_spatial_step();
    void output_results() const;

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

    AffineConstraints<std::complex<double>> constraints;

    SparsityPattern                    sparsity_pattern;
    SparseMatrix<std::complex<double>> system_matrix;
    SparseMatrix<std::complex<double>> rhs_matrix;

    Vector<std::complex<double>> solution;
    Vector<std::complex<double>> system_rhs;

    double       time;
    double       time_step;
    unsigned int timestep_number;

    double kappa;
  };

//  @sect3{Equation data}

// 在我们继续填写主类的细节之前，让我们定义与问题相对应的方程数据，即初始值，以及一个右手类。我们将把初始条件也用于边界值，我们只是保持边界值不变）。我们使用派生自Function类模板的类来做这件事，这个模板之前已经用过很多次了，所以下面的内容看起来并不令人惊讶。唯一值得注意的是，我们这里有一个复值问题，所以我们必须提供Function类的第二个模板参数（否则会默认为`double`）。此外，`value()`函数的返回类型当然也是复数。

// 这些函数精确地返回什么，在介绍部分的最后已经讨论过了。

  template <int dim>
  class InitialValues : public Function<dim, std::complex<double>>
  {
  public:
    InitialValues()
      : Function<dim, std::complex<double>>(1)
    {}

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

  template <int dim>
  std::complex<double>
  InitialValues<dim>::value(const Point<dim> & p,
                            const unsigned int component) const
  {
    static_assert(dim == 2, "This initial condition only works in 2d.");

    (void)component;
    Assert(component == 0, ExcIndexRange(component, 0, 1));

    const std::vector<Point<dim>> vortex_centers = {{0, -0.3},
                                                    {0, +0.3},
                                                    {+0.3, 0},
                                                    {-0.3, 0}};

    const double R = 0.1;
    const double alpha =
      1. / (std::pow(R, dim) * std::pow(numbers::PI, dim / 2.));

    double sum = 0;
    for (const auto &vortex_center : vortex_centers)
      {
        const Tensor<1, dim> distance = p - vortex_center;
        const double         r        = distance.norm();

        sum += alpha * std::exp(-(r * r) / (R * R));
      }

    return {std::sqrt(sum), 0.};
  }

  template <int dim>
  class Potential : public Function<dim>
  {
  public:
    Potential() = default;
    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override;
  };

  template <int dim>
  double Potential<dim>::value(const Point<dim> & p,
                               const unsigned int component) const
  {
    (void)component;
    Assert(component == 0, ExcIndexRange(component, 0, 1));

    return (Point<dim>().distance(p) > 0.7 ? 1000 : 0);
  }

//  @sect3{Implementation of the NonlinearSchroedingerEquation class}

// 我们首先指定了类的构造函数的实现。

  template <int dim>
  NonlinearSchroedingerEquation<dim>::NonlinearSchroedingerEquation()
    : fe(2)
    , dof_handler(triangulation)
    , time(0)
    , time_step(1. / 128)
    , timestep_number(0)
    , kappa(1)
  {}
// @sect4{Setting up data structures and assembling matrices}

// 下一个函数是在程序开始时，也就是在第一个时间步骤之前，设置网格、DoFHandler以及矩阵和向量。如果你已经阅读了至少到 step-6 为止的教程程序，那么前几行是相当标准的。

  template <int dim>
  void NonlinearSchroedingerEquation<dim>::setup_system()
  {
    GridGenerator::hyper_cube(triangulation, -1, 1);
    triangulation.refine_global(6);

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

    dof_handler.distribute_dofs(fe);

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

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

    system_matrix.reinit(sparsity_pattern);
    rhs_matrix.reinit(sparsity_pattern);

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

    constraints.close();
  }

// 接下来，我们组装相关的矩阵。按照我们对斯特朗分裂的空间步骤（即每个时间步骤中三个部分步骤中的第二个步骤）的Crank-Nicolson离散化的写法，我们被引导到线性系统
// $\left[ -iM  +  \frac 14 k_{n+1} A + \frac 12 k_{n+1} W \right]
//    \Psi^{(n,2)}
//   =
//   \left[ -iM  -  \frac 14 k_{n+1} A - \frac 12 k_{n+1} W \right]
//    \Psi^{(n,1)}$

//     换句话说，这里有两个矩阵在起作用--一个用于左手边，一个用于右手边。我们分别建立这些矩阵。我们可以避免建立右手边的矩阵，而只是在每个时间步长中形成矩阵的*作用* $\Psi^{(n,1)}$ 。这可能更有效，也可能不有效，但是对于这个程序来说，效率并不是最重要的）。)

  template <int dim>
  void NonlinearSchroedingerEquation<dim>::assemble_matrices()
  {
    const QGauss<dim> quadrature_formula(fe.degree + 1);

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

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

    FullMatrix<std::complex<double>> cell_matrix_lhs(dofs_per_cell,
                                                     dofs_per_cell);
    FullMatrix<std::complex<double>> cell_matrix_rhs(dofs_per_cell,
                                                     dofs_per_cell);

    std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);
    std::vector<double>                  potential_values(n_q_points);
    const Potential<dim>                 potential;

    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        cell_matrix_lhs = std::complex<double>(0.);
        cell_matrix_rhs = std::complex<double>(0.);

        fe_values.reinit(cell);

        potential.value_list(fe_values.get_quadrature_points(),
                             potential_values);

        for (unsigned int q_index = 0; q_index < n_q_points; ++q_index)
          {
            for (unsigned int k = 0; k < dofs_per_cell; ++k)
              {
                for (unsigned int l = 0; l < dofs_per_cell; ++l)
                  {
                    const std::complex<double> i = {0, 1};

                    cell_matrix_lhs(k, l) +=
                      (-i * fe_values.shape_value(k, q_index) *
                         fe_values.shape_value(l, q_index) +
                       time_step / 4 * fe_values.shape_grad(k, q_index) *
                         fe_values.shape_grad(l, q_index) +
                       time_step / 2 * potential_values[q_index] *
                         fe_values.shape_value(k, q_index) *
                         fe_values.shape_value(l, q_index)) *
                      fe_values.JxW(q_index);

                    cell_matrix_rhs(k, l) +=
                      (-i * fe_values.shape_value(k, q_index) *
                         fe_values.shape_value(l, q_index) -
                       time_step / 4 * fe_values.shape_grad(k, q_index) *
                         fe_values.shape_grad(l, q_index) -
                       time_step / 2 * potential_values[q_index] *
                         fe_values.shape_value(k, q_index) *
                         fe_values.shape_value(l, q_index)) *
                      fe_values.JxW(q_index);
                  }
              }
          }

        cell->get_dof_indices(local_dof_indices);
        constraints.distribute_local_to_global(cell_matrix_lhs,
                                               local_dof_indices,
                                               system_matrix);
        constraints.distribute_local_to_global(cell_matrix_rhs,
                                               local_dof_indices,
                                               rhs_matrix);
      }
  }
// @sect4{Implementing the Strang splitting steps}

// 在建立了上述所有数据结构后，我们现在可以实现构成斯特朗分裂方案的部分步骤。我们从推进阶段的半步开始，这被用作每个时间步骤的第一和最后部分。

// 为此，回顾一下，对于第一个半步，我们需要计算  $\psi^{(n,1)} = e^{-i\kappa|\psi^{(n,0)}|^2 \tfrac 12\Delta t} \; \psi^{(n,0)}$  。这里， $\psi^{(n,0)}=\psi^{(n)}$ 和 $\psi^{(n,1)}$ 是空间的函数，分别对应于前一个完整时间步骤的输出和三个部分步骤中第一个步骤的结果。必须为第三个部分步骤计算相应的解决方案，即  $\psi^{(n,3)} = e^{-i\kappa|\psi^{(n,2)}|^2 \tfrac 12\Delta t} \; \psi^{(n,2)}$  ，其中  $\psi^{(n,3)}=\psi^{(n+1)}$  是整个时间步骤的结果，其输入  $\psi^{(n,2)}$  是斯特朗分割的空间步骤的结果。

// 一个重要的认识是，虽然 $\psi^{(n,0)}(\mathbf x)$ 可能是一个有限元函数（即，是片状多项式），但对于我们使用指数因子更新相位的 "旋转 "函数来说，不一定是这样的（回顾一下，该函数的振幅在该步骤中保持不变）。换句话说，我们可以在每一个点 $\psi^{(n,1)}(\mathbf x)$ *计算 $\mathbf x\in\Omega$ ，但我们不能在网格上表示它，因为它不是一个片状多项式函数。在一个离散的环境中，我们能做的最好的事情就是计算一个投影或内插。换句话说，我们可以计算 $\psi_h^{(n,1)}(\mathbf x) = \Pi_h \left(e^{-i\kappa|\psi_h^{(n,0)}(\mathbf x)|^2 \tfrac 12\Delta t} \; \psi_h^{(n,0)}(\mathbf x) \right)$ ，其中 $\Pi_h$ 是一个投影或内插算子。如果我们选择插值，情况就特别简单。那么，我们需要计算的就是*在节点点上的右手边的值，并将这些作为自由度向量 $\Psi^{(n,1)}$ 的节点值。这很容易做到，因为在这里使用的拉格朗日有限元的节点点上评估右手边，需要我们只看节点向量的一个（复值）条目。换句话说，我们需要做的是计算 $\Psi^{(n,1)}_j = e^{-i\kappa|\Psi^{(n,0)}_j|^2 \tfrac 12\Delta t} \; \Psi^{(n,0)}_j$ ，其中 $j$ 在我们的解向量的所有条目上循环。这就是下面的函数所做的--事实上，它甚至没有为 $\Psi^{(n,0)}$ 和 $\Psi^{(n,1)}$ 使用单独的向量，而只是适当地更新同一个向量。

  template <int dim>
  void NonlinearSchroedingerEquation<dim>::do_half_phase_step()
  {
    for (auto &value : solution)
      {
        const std::complex<double> i         = {0, 1};
        const double               magnitude = std::abs(value);

        value = std::exp(-i * kappa * magnitude * magnitude * (time_step / 2)) *
                value;
      }
  }

// 下一步是求解每个时间步骤中的线性系统，即我们使用的Strang分割的后半步。记得它的形式是 $C\Psi^{(n,2)} = R\Psi^{(n,1)}$ ，其中 $C$ 和 $R$ 是我们之前组装的矩阵。

// 我们在这里解决这个问题的方法是使用直接求解器。我们首先使用 $r=R\Psi^{(n,1)}$ 函数形成右边的 SparseMatrix::vmult() ，并将结果放入`system_rhs`变量。然后我们调用 SparseDirectUMFPACK::solver() ，该函数以矩阵 $C$ 和右手边的向量为参数，并在同一向量`system_rhs`中返回解。最后一步是将计算出的解放回`solution`变量中。

  template <int dim>
  void NonlinearSchroedingerEquation<dim>::do_full_spatial_step()
  {
    rhs_matrix.vmult(system_rhs, solution);

    SparseDirectUMFPACK direct_solver;
    direct_solver.solve(system_matrix, system_rhs);

    solution = system_rhs;
  }

//  @sect4{Creating graphical output}

// 我们应该讨论的最后一个辅助函数和类是那些创建图形输出的函数。对斯特朗分裂的局部和空间部分运行半步和全步的结果是，我们在每个时间步数结束时将`solution`向量 $\Psi^n$ 更新为正确的值。它的条目包含有限元网格节点上的解的复数。

// 复数不容易被视觉化。我们可以输出它们的实部和虚部，即字段 $\text{Re}(\psi_h^{(n)}(\mathbf x))$ 和 $\text{Im}(\psi_h^{(n)}(\mathbf x))$ ，这正是DataOut类在通过 DataOut::add_data_vector() 附加复数向量，然后调用 DataOut::build_patches(). 时所做的事情，这确实是我们下面要做的。

// 但很多时候，我们对解向量的实部和虚部并不特别感兴趣，而是对解的幅度 $|\psi|$ 和相位角 $\text{arg}(\psi)$ 等衍生量感兴趣。在这里这样的量子系统的背景下，幅度本身并不那么有趣，相反，"振幅"， $|\psi|^2$ 才是一个物理属性：它对应于在一个特定的状态场所找到一个粒子的概率密度。将计算出的量放入输出文件以实现可视化的方法--正如在以前的许多教程程序中使用的那样--是使用数据后处理程序和派生类的设施。具体来说，一个复数的振幅和它的相位角都是标量，因此DataPostprocessorScalar类是我们要做的正确工具。

// 因此，我们在这里要做的是实现两个类`ComplexAmplitude`和`ComplexPhase`，为DataOut决定生成输出的每个点计算解决方案的振幅 $|\psi_h|^2$ 和相位 $\text{arg}(\psi_h)$ ，以便进行可视化。下面有大量的模板代码，这两个类中的第一个唯一有趣的部分是它的`evaluate_vector_field()`函数如何计算`computed_quantities`对象。

//（还有一个相当尴尬的事实是，std::norm()函数并没有计算人们天真的想象，即 $|\psi|$  ，而是返回 $|\psi|^2$ 。一个标准函数以这样的方式被错误地命名，这当然是相当令人困惑的......)

  namespace DataPostprocessors
  {
    template <int dim>
    class ComplexAmplitude : public DataPostprocessorScalar<dim>
    {
    public:
      ComplexAmplitude();

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

    template <int dim>
    ComplexAmplitude<dim>::ComplexAmplitude()
      : DataPostprocessorScalar<dim>("Amplitude", update_values)
    {}

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

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

          const std::complex<double> psi(inputs.solution_values[q](0),
                                         inputs.solution_values[q](1));
          computed_quantities[q](0) = std::norm(psi);
        }
    }

// 这些后处理程序类中的第二个是计算每一个点的复值解决方案的相位角。换句话说，如果我们表示  $\psi(\mathbf x,t)=r(\mathbf x,t) e^{i\varphi(\mathbf x,t)}$  ，那么这个类就会计算  $\varphi(\mathbf x,t)$  。函数 std::arg 为我们做这个，并将角度作为实数返回  $-\pi$  和  $+\pi$  之间。

// 由于我们将在结果部分详细解释的原因，我们实际上没有在产生输出的每个位置输出这个值。相反，我们取相位所有评估点的最大值，然后用这个最大值填充每个评估点的输出字段--实质上，我们将相位角作为一个片状常数字段输出，其中每个单元都有自己的常数值。一旦你读完下面的讨论就会明白其中的原因。

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

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

    template <int dim>
    ComplexPhase<dim>::ComplexPhase()
      : DataPostprocessorScalar<dim>("Phase", update_values)
    {}

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

      double max_phase = -numbers::PI;
      for (unsigned int q = 0; q < computed_quantities.size(); ++q)
        {
          Assert(computed_quantities[q].size() == 1,
                 ExcDimensionMismatch(computed_quantities[q].size(), 1));
          Assert(inputs.solution_values[q].size() == 2,
                 ExcDimensionMismatch(inputs.solution_values[q].size(), 2));

          max_phase =
            std::max(max_phase,
                     std::arg(
                       std::complex<double>(inputs.solution_values[q](0),
                                            inputs.solution_values[q](1))));
        }

      for (auto &output : computed_quantities)
        output(0) = max_phase;
    }

  } // namespace DataPostprocessors

// 在这样实现了这些后处理程序后，我们像往常一样创建输出。与其他许多时间相关的教程程序一样，我们给DataOut附加标志，表示时间步数和当前模拟时间。

  template <int dim>
  void NonlinearSchroedingerEquation<dim>::output_results() const
  {
    const DataPostprocessors::ComplexAmplitude<dim> complex_magnitude;
    const DataPostprocessors::ComplexPhase<dim>     complex_phase;

    DataOut<dim> data_out;

    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution, "Psi");
    data_out.add_data_vector(solution, complex_magnitude);
    data_out.add_data_vector(solution, complex_phase);
    data_out.build_patches();

    data_out.set_flags(DataOutBase::VtkFlags(time, timestep_number));

    const std::string filename =
      "solution-" + Utilities::int_to_string(timestep_number, 3) + ".vtu";
    std::ofstream output(filename);
    data_out.write_vtu(output);
  }

//  @sect4{Running the simulation}

// 剩下的步骤是我们如何设置这个程序的整体逻辑。这其实是比较简单的。设置数据结构；将初始条件插值到有限元空间；然后迭代所有时间步长，在每个时间步长上执行斯特朗分割法的三个部分。每隔10个时间步长，我们就生成图形输出。这就是了。

  template <int dim>
  void NonlinearSchroedingerEquation<dim>::run()
  {
    setup_system();
    assemble_matrices();

    time = 0;
    VectorTools::interpolate(dof_handler, InitialValues<dim>(), solution);
    output_results();

    const double end_time = 1;
    for (; time <= end_time; time += time_step)
      {
        ++timestep_number;

        std::cout << "Time step " << timestep_number << " at t=" << time
                  << std::endl;

        do_half_phase_step();
        do_full_spatial_step();
        do_half_phase_step();

        if (timestep_number % 1 == 0)
          output_results();
      }
  }
} // namespace Step58

//  @sect4{The main() function}

// 其余的又是锅炉板，和以前几乎所有的教程程序完全一样。

int main()
{
  try
    {
      using namespace Step58;

      NonlinearSchroedingerEquation<2> nse;
      nse.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;
}


#pragma once

#include <Eigen/Dense>

using Eigen::MatrixXd;
using Eigen::Ref;
using Eigen::RowVectorXd;

class Scaler {
public:
    MatrixXd fit_transform(const Ref<const MatrixXd> input);

    MatrixXd transform(const Ref<const MatrixXd> input) const;

private:
    RowVectorXd std;
    RowVectorXd mean;
};


#include "../linfereactdiff.h"

#include <gtest/gtest.h>
#include <lf/assemble/assemble.h>
#include <lf/base/base.h>
#include <lf/fe/fe.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <memory>
#include <utility>

namespace LinFeReactDiff::test {

constexpr char mesh_file[] = CURRENT_SOURCE_DIR "/../../meshes/square.msh";

TEST(LinFeReactDiff, TestSolveFe) {
  auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader(std::move(mesh_factory), mesh_file);
  auto mesh = reader.mesh();
  Eigen::VectorXd mu = solveFE(mesh);
  ASSERT_NEAR(mu(mu.size() - 1), 0.00254543, 0.00001);
  ASSERT_NEAR(mu(mu.size() - 2), 0.00145535, 0.00001);
}

TEST(LinFeReactDiff, TestEnergy) {
  auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader(std::move(mesh_factory), mesh_file);
  auto mesh = reader.mesh();

  // Implementation from solution to not depend on the first task
  auto zero = [](Eigen::Vector2d x) -> double { return 0.; };
  lf::mesh::utils::MeshFunctionGlobal mf_zero{zero};
  auto identity = [](Eigen::Vector2d x) -> double { return 1.; };
  lf::mesh::utils::MeshFunctionGlobal mf_identity{identity};
  auto c = [](Eigen::Vector2d x) -> double { return x[0] * x[1]; };
  lf::mesh::utils::MeshFunctionGlobal mf_c{c};

  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh);
  const lf::mesh::Mesh &mesh_p{*(fe_space->Mesh())};
  const lf::assemble::DofHandler &dofh{fe_space->LocGlobMap()};

  const lf::base::size_type N_dofs(dofh.NumDofs());
  lf::assemble::COOMatrix<double> A(N_dofs, N_dofs);
  lf::uscalfe::ReactionDiffusionElementMatrixProvider<
      double, decltype(mf_identity), decltype(mf_zero)>
      elmat_builder(fe_space, mf_identity, mf_zero);
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, elmat_builder, A);
  Eigen::Matrix<double, Eigen::Dynamic, 1> phi(N_dofs);
  phi.setZero();

  lf::uscalfe::ScalarLoadElementVectorProvider<double, decltype(mf_c)>
      elvec_builder(fe_space, mf_c);

  AssembleVectorLocally(0, dofh, elvec_builder, phi);

  const lf::fe::ScalarReferenceFiniteElement<double> *rsf_edge_p =
      fe_space->ShapeFunctionLayout(lf::base::RefEl::kSegment());
  LF_ASSERT_MSG(rsf_edge_p != nullptr, "FE specification for edges missing");
  auto bd_flags{lf::mesh::utils::flagEntitiesOnBoundary(fe_space->Mesh(), 1)};
  auto ess_bdc_flags_values_findest{lf::fe::InitEssentialConditionFromFunction(
      *fe_space,
      [&bd_flags](const lf::mesh::Entity &edge) -> bool {
        return bd_flags(edge);
      },
      mf_zero)};

  lf::assemble::FixFlaggedSolutionComponents<double>(
      [&ess_bdc_flags_values_findest](lf::assemble::glb_idx_t gdof_idx) {
        return ess_bdc_flags_values_findest[gdof_idx];
      },
      A, phi);

  Eigen::VectorXd mu;
  Eigen::SparseMatrix<double> A_crs = A.makeSparse();
  Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
  solver.compute(A_crs);
  mu = solver.solve(phi);

  // compute energy
  double energy = computeEnergy(mesh, mu);
  ASSERT_NEAR(energy, 0.0105153, 0.00001);
}

}  // namespace LinFeReactDiff::test


/*
   Based on https://github.com/jackjack-jj/jeeq, GPLv3.
   Specifically designed to use Smileycoin key pairs for encoding/decoding so
   this is not as general as jeeq.py
   All the math here is explained pretty well on this wikipedia page:
   https://en.wikipedia.org/wiki/ElGamal_encryption
 */
#include <string.h>
#include <stdbool.h>
#include <boost/endian/conversion.hpp>

#include <vector>
#include <stdexcept>

#include <openssl/ec.h>
#include <openssl/bn.h>
#include <openssl/obj_mac.h>
#include <openssl/crypto.h>
#include <openssl/rand.h>
#include <openssl/sha.h>
#include <openssl/err.h>

#include "key.h"
#include "util.h"
#include "jeeq.h"

#define PRIVHEADER_LEN          9
#define PUBHEADER_LEN           7
#define PRIVKEY_LEN             32
#define COMPR_PUBKEY_LEN        33
#define UNCOMPR_PUBKEY_LEN      65
#define CHUNK_SIZE              32
#define VERSION                 0x00
#define GX_HEX                  "79BE667EF9DCBBAC55A06295CE870B07029BFCDB2DCE28D959F2815B16F81798"
#define GY_HEX                  "483ADA7726A3C4655DA4FBFC0E1108A8FD17B448A68554199C47D08FFB10D4B8"
#define GORDER_HEX              "FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFEBAAEDCE6AF48A03BBFD25E8CD0364141"
#define GCOFACTOR_HEX           "01"

using namespace boost::endian;

/* return:  secp256k1 curve with the bitcoin generator, order and cofactor
 * in:      ctx, bignum context
 */
static EC_GROUP *init_curve(BN_CTX *ctx)
{
    EC_GROUP *group = EC_GROUP_new_by_curve_name(NID_secp256k1);

    EC_POINT *generator = EC_POINT_new(group);

    BIGNUM *Gx = BN_new();
    BIGNUM *Gy = BN_new();
    BIGNUM *order = BN_new();
    BIGNUM *cofactor = BN_new();

    BN_hex2bn(&Gx, GX_HEX);
    BN_hex2bn(&Gy, GY_HEX);

    BN_hex2bn(&order, GORDER_HEX);
    BN_hex2bn(&cofactor, GCOFACTOR_HEX);

    EC_POINT_set_affine_coordinates_GFp(group, generator, Gx, Gy, ctx);
    EC_GROUP_set_generator(group, generator, order, cofactor);

    BN_free(Gx);
    BN_free(Gy);
    BN_free(cofactor);
    BN_free(order);

    EC_POINT_free(generator);

    return group;
}

/* return: 1
 * out: m, pointer to message that will be prefaced with the private header
 * in:  nmsg, pointer to the message
 *      msg_length, length of the message
 */
static int write_private_header(uint8_t *m, const uint8_t *nmsg, const uint32_t msg_length)
{
    uint8_t hash[SHA256_DIGEST_LENGTH];
    SHA256(nmsg, msg_length, hash);

    m[0] = VERSION;
    m[1] = 0x00;
    m[2] = 0x06;
    *(uint32_t*)&m[3] = native_to_big(msg_length);
    m[7] = hash[0];
    m[8] = hash[1];

    return 1;
}

/* return: 1
 * out:     enc, the encrypted string to preface the public header with
 * in:      pub, pointer to raw pubkey,
 *          is_compressed, whether the pubkey is compressed NEEDED
 */
static int write_public_header(uint8_t *enc, const uint8_t *pub)
{
    uint8_t hash[SHA256_DIGEST_LENGTH];
    size_t pubkey_length = (pub[0] == 0x04) ? UNCOMPR_PUBKEY_LEN : COMPR_PUBKEY_LEN;

    SHA256(pub, pubkey_length, hash);

    enc[0] = 0x6a;
    enc[1] = 0x6a;
    enc[2] = VERSION;
    enc[3] = 0x00;
    enc[4] = 0x02;
    enc[5] = hash[0];
    enc[6] = hash[1];

    return 1;
}

/* return:  1 on success, 0 otherwise
 * out:     message_length, read message length
 * in:      dec, decrypted string
 */
static int read_private_header(size_t *message_length, const uint8_t *dec)
{
    if (dec[0] != VERSION) return 0;
    if (dec[1] != 0x00) return 0;
    if (dec[2] != 0x06) return 0;

    uint32_t msg_len = big_to_native(*(uint32_t*)&dec[3]);

    /* com_mh stands for computed message hash */
    uint8_t com_mh[SHA256_DIGEST_LENGTH];
    SHA256(&dec[PRIVHEADER_LEN], msg_len, com_mh);

    /* check that our computed hash matches the one in the header */
    if (dec[7] != com_mh[0] || dec[8] != com_mh[1]) return 0;
    *message_length = msg_len;

    return 1;
}

/* return: 1 on success, 0 otherwise
 * in:  group, bitcoin curve
 *      enc, encryted string
 *      bn_privkey, our privkey as a bignum
 *      is_compressed??,  RETHINK
 *      ctx, bignum context
 */
static int read_public_header(const EC_GROUP *group, const uint8_t *enc,
        const uint8_t* our_pubkey, BN_CTX *ctx)
{
    if (enc[0] != 0x6a)     return 0;
    if (enc[1] != 0x6a)     return 0;
    if (enc[2] != VERSION)  return 0;
    if (enc[3] != 0x00)     return 0;
    if (enc[4] != 0x02)     return 0;

    /* computed public key hash = com_pkh */
    uint8_t com_pkh[SHA256_DIGEST_LENGTH];
    SHA256(our_pubkey, (our_pubkey[0]==0x04 ? UNCOMPR_PUBKEY_LEN : COMPR_PUBKEY_LEN), com_pkh);

    if (enc[5] != com_pkh[0] || enc[6] != com_pkh[1])
        return 0;

    return 1;
}

/* return: 1 on success, 0 otherwise
 * out: y, the y value corresponding to x+offset, if it is found
 *      offset, the offset needed to the x value to give a y
 * in:  group, bitcoin curve
 *      x,   our x
 *      odd, whether we want the y value to be odd or even,
 *      ctx, bignum context
 */
static int y_from_x(const EC_GROUP *group, BIGNUM *y, size_t *offset, const BIGNUM *x, const bool odd, BN_CTX *ctx)
{
    EC_POINT *M = EC_POINT_new(group);

    /* try to find y the easy way */
    if (EC_POINT_set_compressed_coordinates_GFp(group, M, x, odd, ctx) == 1)
    {
        EC_POINT_get_affine_coordinates_GFp(group, M, NULL, y, ctx);
        *offset = 0;
        EC_POINT_free(M);
        return 1;
    }

    int ret = 0;
    BN_CTX_start(ctx);

    BIGNUM *p = BN_CTX_get(ctx);
    BIGNUM *a = BN_CTX_get(ctx);
    BIGNUM *b = BN_CTX_get(ctx);

    EC_GROUP_get_curve_GFp(group, p, a, b, ctx);

    BIGNUM *Mx = BN_CTX_get(ctx);
    BN_copy(Mx, x);
    BIGNUM *My = BN_CTX_get(ctx);
    BIGNUM *My2 = BN_CTX_get(ctx);
    BIGNUM *aMx2 = BN_CTX_get(ctx);

    BIGNUM *half = BN_CTX_get(ctx);
    BN_copy(half, p);
    BN_add_word(half, 1);
    BN_div_word(half, 4);

    /* xoffset can be in the range 1-127 since we have 7 bits free,
       we only need 1 bit to discern odd from even points
    */
    for (int i = 1; i < 128; i++)
    {
        BN_add_word(Mx, 1);

        /* My2 = (Mx^2 * Mx mod p) */
        BN_sqr(My2, Mx, ctx);
        BN_mod_mul(My2, My2, Mx, p, ctx);

        BN_mod_sqr(aMx2, Mx, p, ctx);
        BN_mul(aMx2, aMx2, a, ctx);

        BN_mod(b, b, p, ctx);

        BN_add(My2, My2, aMx2);
        BN_add(My2, My2, b);

        BN_mod_exp(My, My2, half, p, ctx);

        /* this function will return 1 on success (point on curve), else 0 */
        if (EC_POINT_set_affine_coordinates_GFp(group, M, Mx, My, ctx) == 1)
        {
            if (odd == BN_is_bit_set(My, 0))
            {
                BN_copy(y, My);
                *offset = i;
            }
            else
            {
                BN_sub(y, p, My);
                *offset = i;
            }

            ret = 1;
            break;
        }
    }

    /* some errors here are expected since set_affine_coordinates logs an error
     * when we try to set an invalid x,y combination when trying offsets
     */
    if (ERR_peek_error())
    {
        unsigned long e = 0;
        while ((e = ERR_get_error()))
            LogPrintf("y_from_x %s: %s", ERR_func_error_string(e), ERR_reason_error_string(e));
    }

    BN_CTX_end(ctx);
    EC_POINT_free(M);

    return ret;
}

/* return:  a malloc pointer to the encrypted string,
 * out:     enc_len, size of the encrypted string in bytes
 * in:      pubkey, pointer to raw pubkey
 *          msg, pointer to the message to be encrypted
 *          msg_len, length of the message to be encrypted
 */
static uint8_t *encrypt_message(size_t *enc_len, const uint8_t *pubkey,
        const uint8_t *msg, const size_t msg_len)
{
    BN_CTX *ctx = BN_CTX_new();
    /* our secp256k1 curve and bitcoin generator
     * these must be thread local as openssl does not support shared
     * use of its data structures
     */
    EC_GROUP *group = init_curve(ctx);

    uint8_t *ret = NULL;

    EC_POINT *pk = EC_POINT_new(group);

    // get so many blocks of 32B blocks that msg will fit
    int chunk_count = (PRIVHEADER_LEN + msg_len)/CHUNK_SIZE + 1;

    uint8_t *m = (uint8_t*)OPENSSL_zalloc(chunk_count * CHUNK_SIZE);

    write_private_header(m, msg, msg_len);
    memcpy(&m[PRIVHEADER_LEN], msg, msg_len);

    BIGNUM *bn_pubkey = BN_new();
    BN_bin2bn(&pubkey[1], 32, bn_pubkey);

    // pubkey is compressed
    if (pubkey[0] == 0x02 || pubkey[0] == 0x03)
    {
        EC_POINT_set_compressed_coordinates_GFp(group, pk, bn_pubkey, pubkey[0]==0x03, ctx);
    }
    else
    {
        BIGNUM *bn_pubkey_extra = BN_new();

        BN_bin2bn(&pubkey[1+32], 32, bn_pubkey_extra);
        EC_POINT_set_affine_coordinates_GFp(group, pk, bn_pubkey, bn_pubkey_extra, ctx);

        BN_free(bn_pubkey_extra);
    }

    BIGNUM *rand = BN_new();
    BIGNUM *rand_range = BN_new();
    BIGNUM *Mx = BN_new();
    BIGNUM *My = BN_new();
    EC_POINT *M = EC_POINT_new(group);
    EC_POINT *T = EC_POINT_new(group);
    EC_POINT *U = EC_POINT_new(group);
    EC_GROUP_get_order(group, rand_range, ctx);
    BN_sub_word(rand_range, 1);

    uint8_t *enc = (uint8_t*)malloc(PUBHEADER_LEN + chunk_count * 2*COMPR_PUBKEY_LEN);
    write_public_header(enc, pubkey);
    int enc_loc = PUBHEADER_LEN;
    int m_loc = 0;
    size_t xoffset = 0;

    for (int i = 0; i < chunk_count; i++)
    {
        /* since rand must be in [1,...,q-1] */
        BN_rand_range(rand, rand_range);
        BN_add_word(rand, 1);

        if (!BN_bin2bn(&m[m_loc], CHUNK_SIZE, Mx))
            goto err;

        if (!y_from_x(group, My, &xoffset, Mx, true, ctx))
            goto err;

        /* adding our xoffset that we get from y_from_x() */
        BN_add_word(Mx, xoffset);

        EC_POINT_set_affine_coordinates_GFp(group, M, Mx, My, ctx);

        /* see wiki */
        EC_POINT_mul(group, T, rand, NULL, NULL, ctx);
        EC_POINT_mul(group, U, NULL, pk, rand, ctx);
        EC_POINT_add(group, U, U, M, ctx);

        EC_POINT_point2oct(group, T, POINT_CONVERSION_COMPRESSED,
                &enc[enc_loc], COMPR_PUBKEY_LEN, ctx);
        EC_POINT_point2oct(group, U, POINT_CONVERSION_COMPRESSED,
                &enc[enc_loc+COMPR_PUBKEY_LEN], COMPR_PUBKEY_LEN, ctx);

        /* encoding our offset within the odd/even byte (02/03), the first bit represets
         * evenness and other 7 represent the offset */
        enc[enc_loc] = enc[enc_loc] - 2 + (xoffset << 1);

        enc_loc += 2*COMPR_PUBKEY_LEN;
        m_loc += CHUNK_SIZE;
    }

    /* success */
    *enc_len = enc_loc;
    ret = enc;

err:
    if (ERR_peek_error())
    {
        unsigned long e = 0;
        while ((e = ERR_get_error()))
            LogPrintf("encrypt_message %s: %s", ERR_func_error_string(e), ERR_reason_error_string(e));
    }

    OPENSSL_clear_free(m, chunk_count * CHUNK_SIZE);
    BN_free(rand);
    BN_free(rand_range);
    BN_free(Mx);
    BN_free(My);
    BN_free(bn_pubkey);
    EC_POINT_free(M);
    EC_POINT_free(T);
    EC_POINT_free(U);
    EC_POINT_free(pk);

    EC_GROUP_free(group);
    BN_CTX_free(ctx);

    return ret;
}

/* return:  malloc pointer to the decrypted message
 * out:     dec_len, length of the decrypted message
 * in:      privkey, pointer to raw private key
 *          pubkey, pointer to the raw public key matching privkey
 *          enc, pointer to the encrypted message
 *          enc_len, length of the encrypted message
 */
static uint8_t *decrypt_message(size_t *dec_len, const uint8_t *privkey, const uint8_t *pubkey,
                                const uint8_t *enc, const size_t enc_len)
{
    BN_CTX *ctx = BN_CTX_new();
    EC_GROUP *group = init_curve(ctx);

    /* check that the public header is valid and matches our priv/pubkey pair */
    if (!read_public_header(group, enc, pubkey, ctx))
        return NULL;

    int chunk_count = (enc_len - PUBHEADER_LEN) / (2*COMPR_PUBKEY_LEN);

    uint8_t *r = (uint8_t*)OPENSSL_malloc(PRIVHEADER_LEN + chunk_count * CHUNK_SIZE);

    uint8_t *Tser = (uint8_t*)OPENSSL_malloc(COMPR_PUBKEY_LEN);
    uint8_t *User = (uint8_t*)OPENSSL_malloc(COMPR_PUBKEY_LEN);
    int xoffset = 0;

    EC_POINT *T = EC_POINT_new(group);
    EC_POINT *U = EC_POINT_new(group);
    EC_POINT *M = EC_POINT_new(group);
    EC_POINT *V = EC_POINT_new(group);

    BIGNUM *Mx = BN_new();

    BIGNUM *bn_privkey = BN_new();
    BN_bin2bn(privkey, PRIVKEY_LEN, bn_privkey);

    int enc_loc = PUBHEADER_LEN;
    int r_loc = 0;
    for (int i = 0; i < chunk_count; i++)
    {
        memcpy(Tser, &enc[enc_loc], COMPR_PUBKEY_LEN);
        memcpy(User, &enc[enc_loc+COMPR_PUBKEY_LEN], COMPR_PUBKEY_LEN);

        /* decode the offset and evenness from the first byte */
        xoffset = Tser[0] >> 1;
        Tser[0] = 2 + (Tser[0]&1);

        EC_POINT_oct2point(group, T, Tser, COMPR_PUBKEY_LEN, ctx);
        EC_POINT_oct2point(group, U, User, COMPR_PUBKEY_LEN, ctx);

        EC_POINT_mul(group, V, NULL, T, bn_privkey, ctx);
        EC_POINT_invert(group, V, ctx);
        EC_POINT_add(group, M, U, V, ctx);

        EC_POINT_get_affine_coordinates_GFp(group, M, Mx, NULL, ctx);

        /* substract our offset so that we get the original Mx */
        BN_sub_word(Mx, xoffset);

        BN_bn2binpad(Mx, &r[r_loc], CHUNK_SIZE);

        r_loc += CHUNK_SIZE;
        enc_loc += 2*COMPR_PUBKEY_LEN;
    }

    uint8_t *ret = NULL;

    size_t size = 0;
    if (!read_private_header(&size, r))
        goto err;

    /* success */
    ret = (uint8_t*)malloc(size);
    memcpy(ret, &r[PRIVHEADER_LEN], size);
    *dec_len = size;

err:
    if (ERR_peek_error())
    {
        unsigned long e = 0;
        while ((e = ERR_get_error()))
            LogPrintf("decrypt_message %s: %s", ERR_func_error_string(e), ERR_reason_error_string(e));
    }

    OPENSSL_free(Tser);
    OPENSSL_free(User);

    EC_POINT_clear_free(V);
    EC_POINT_clear_free(U);
    EC_POINT_clear_free(M);
    EC_POINT_clear_free(T);

    BN_free(Mx);
    BN_clear_free(bn_privkey);

    OPENSSL_clear_free(r, r_loc);

    EC_GROUP_free(group);
    BN_CTX_free(ctx);

    return ret;
}

/* wrappers for the C functions
 */
using namespace std;

namespace Jeeq {

vector<uint8_t> EncryptMessage(const CPubKey pubkey, const string msg)
{
    // check pubkey
    if (!pubkey.IsValid())
        throw runtime_error("Jeeq::EncryptMessage(): called with an invalid public key");
    //  ensure that msg is not empty
    if (msg.size() == 0) throw runtime_error("Jeeq::EncryptMessage(): no message to encrypt");

    size_t enc_len = 0;
    uint8_t *benc = encrypt_message(&enc_len, pubkey.begin(), (uint8_t*)&msg[0], msg.size());
    if (benc == NULL || enc_len == 0)
        return vector<uint8_t>{};

    vector<uint8_t> enc(benc, benc+enc_len);
    return enc;
}

string DecryptMessage(const CKey privkey, const vector<uint8_t> enc)
{
    // check privkey
    if (!privkey.IsValid())
        throw runtime_error("Jeeq::DecryptMessage(): called with an invalid private key");
    // ensure that enc is not empty
    if (enc.size() == 0)
        throw runtime_error("Jeeq::DecryptMessage(): no encoded message to decrypt");

    size_t dec_len = 0;
    CPubKey pubkey = privkey.GetPubKey();
    uint8_t *bdec = decrypt_message(&dec_len, privkey.begin(), pubkey.begin(),
                      enc.data(), enc.size());
    if (bdec == NULL || dec_len == 0)
        return string();

    string dec(bdec, bdec+dec_len);
    free(bdec);
    return dec;
}

}

// if we don't do this bignum.h ducks everything up
#include "base58.h"
#include "net.h"
#include "netbase.h"
#include "util.h"
#include "wallet.h"
#include "walletdb.h"

// auxillary function for EncryptMessage
namespace Jeeq {
CPubKey SearchForPubKey(CBitcoinAddress addr)
{
    CScript spk;
    spk.SetDestination(addr.Get());

    CBlock block;
    uint256 blockhash;
    CTransaction txout;

    // beginning at the blockchain tip going back
    for (CBlockIndex* pindex = chainActive.Tip(); pindex && pindex->pprev; pindex = pindex->pprev)
    {
        if (!ReadBlockFromDisk(block, pindex))
            return CPubKey();

        // skip coinbase (vtx[0])
        for (unsigned int i = 1; i < block.vtx.size(); i++)
        {
            for (unsigned int j = 0; j < block.vtx[i].vin.size(); j++)
            {
                // get tx from txid and blockhash and store it in txout
                GetTransaction(block.vtx[i].vin[j].prevout.hash, txout, blockhash);
                int n = block.vtx[i].vin[j].prevout.n;

                if (txout.vout[n].scriptPubKey == spk)
                {
                    CScript ssig = block.vtx[i].vin[j].scriptSig;

                    opcodetype opcode;
                    std::vector<unsigned char> pkdata;
                    auto pc = ssig.begin();

                    // we do it twice since the pubkey is the second item
                    ssig.GetOp(pc, opcode, pkdata);
                    ssig.GetOp(pc, opcode, pkdata);

                    return CPubKey(pkdata.begin(), pkdata.end());
                }
            }
        }
    }

    return CPubKey();
}
} // Namespace Jeeq


/**
 * @file   test.cpp
 * @author Chengyu Liu
 * @date   Tue 08 June 2021
 *
 * @brief  这是个抛物型方程的例子，非常简单，中间就没有解释太多了
 *
 */

#include <iostream>
#include <vector>
#include <iterator>
#include <algorithm>

#include <base/exceptions.h>
#include <lac/full_matrix.h>
#include <lac/sparsity_pattern.h>
#include <lac/sparse_matrix.h>

#include <AFEPack/EasyMesh.h>
#include <AFEPack/TemplateElement.h>
#include <AFEPack/FEMSpace.h>
#include <AFEPack/BilinearOperator.h>
#include <AFEPack/Operator.h>
#include <AFEPack/Geometry.h>
#include <AFEPack/BoundaryCondition.h>

//#include <trace/mintrace.h>
#include <CG/CGSolver.h>
#include <EigenSolver/Miscellaneous.h>
#include <EigenSolver/EigenSolver.h>
#define DIM 2
#define PI (4.0*atan(1.0))

/// 初值和边值的表达式
double _u_(const double * p)
{
  return sin(PI*p[0]) * sin(PI*p[1]);
  //return sin(PI*p[0]) * sin(2*PI*p[1]);
  //return p[0]*exp(p[1]);
}

/// 右端项 In fact, right hand side vector will not be used in Laplacian eigenvalue problems.
double _f_(const double * p)
{
  return 10+5*PI*PI*_u_(p);
  //return p[0]*p[1] + sin(p[1]);
}

double f(const double * p)
{
  return 0;
}

/// Construct the Stiff matrix in the left hand side.
class Stiff_Matrix : public L2InnerProduct<DIM,double>
{
public:
  Stiff_Matrix(FEMSpace<double,DIM>& sp) :
    L2InnerProduct<DIM,double>(sp, sp) {}
  virtual void getElementMatrix(const Element<double,DIM>& e0,
                                const Element<double,DIM>& e1,
                                const ActiveElementPairIterator< DIM >::State s)
  {
    double vol = e0.templateElement().volume();
    u_int acc = algebricAccuracy();
    const QuadratureInfo<DIM>& qi = e0.findQuadratureInfo(acc);
    u_int n_q_pnt = qi.n_quadraturePoint();
    std::vector<double> jac = e0.local_to_global_jacobian(qi.quadraturePoint());
    //AFEPack::Point<DIM> test1;
    //std::vector<AFEPack::Point<DIM>> test2;
    //
    // Here always get an error! vector<Point<DIM> >;
    // Reason: Because there is a same name class in deal.ii: Point class. While
    // using Point, it might use the dealii.Point defaultly!!!! So you just add
    // the namespace to control the class will solve the problem;
    std::vector<AFEPack::Point<DIM>> q_pnt = e0.local_to_global(qi.quadraturePoint());
    //std::cout<<"ATTENTION!This is a test flag!!!"<<std::endl;
    std::vector<std::vector<double> > bas_val = e0.basis_function_value(q_pnt);
    std::vector<std::vector<std::vector<double> > > bas_grad = e0.basis_function_gradient(q_pnt);
    u_int n_ele_dof = e0.dof().size();
    for (u_int l = 0;l < n_q_pnt;++ l) {
      double Jxw = vol*qi.weight(l)*jac[l];
      for (u_int i = 0;i < n_ele_dof;++ i) {
        for (u_int j = 0;j < n_ele_dof;++ j) {
          //elementMatrix(i,j) += Jxw*(bas_val[i][l]*bas_val[j][l]/_dt +
	  //                         innerProduct(bas_grad[i][l], bas_grad[j][l]));
	  elementMatrix(i,j) += Jxw*(innerProduct(bas_grad[i][l], bas_grad[j][l]));
        }
      }
    }
  }
};

// Construct the Mass matrix in the right hand side,
class Mass_Matrix : public L2InnerProduct<DIM,double>
{
public:
  Mass_Matrix(FEMSpace<double,DIM>& sp) :
    L2InnerProduct<DIM,double>(sp, sp){}
  virtual void getElementMatrix(const Element<double,DIM>& e0,
                                const Element<double,DIM>& e1,
                                const ActiveElementPairIterator< DIM >::State s)
  {
    double vol = e0.templateElement().volume();
    u_int acc = algebricAccuracy();
    const QuadratureInfo<DIM>& qi = e0.findQuadratureInfo(acc);
    u_int n_q_pnt = qi.n_quadraturePoint();
    std::vector<double> jac = e0.local_to_global_jacobian(qi.quadraturePoint());
    //AFEPack::Point<DIM> test1;
    //std::vector<AFEPack::Point<DIM>> test2;
    //
    // Here always get an error! vector<Point<DIM> >;
    // Reason: Because there is a same name class in deal.ii: Point class. While
    // using Point, it might use the dealii.Point defaultly!!!! So you just add
    // the namespace to control the class will solve the problem;
    std::vector<AFEPack::Point<DIM>> q_pnt = e0.local_to_global(qi.quadraturePoint());
    //std::cout<<"ATTENTION!This is a test flag!!!"<<std::endl;
    std::vector<std::vector<double> > bas_val = e0.basis_function_value(q_pnt);
    std::vector<std::vector<std::vector<double> > > bas_grad = e0.basis_function_gradient(q_pnt);
    u_int n_ele_dof = e0.dof().size();
    for (u_int l = 0;l < n_q_pnt;++ l) {
      double Jxw = vol*qi.weight(l)*jac[l];
      for (u_int i = 0;i < n_ele_dof;++ i) {
        for (u_int j = 0;j < n_ele_dof;++ j) {
          /*elementMatrix(i,j) += Jxw*(bas_val[i][l]*bas_val[j][l]/_dt +
	    innerProduct(bas_grad[i][l], bas_grad[j][l]));*/
	  elementMatrix(i,j) += Jxw*(bas_val[i][l]*bas_val[j][l]);
        }
      }
    }
  }
};

void boundary_condition_apply(const FEMSpace<double, DIM>& sp,
		SparseMatrix<double>& A,
		BoundaryConditionAdmin<double,DIM> boundary,
		bool preserve_symmetry = true)
{
  u_int n_dof = sp.n_dof();
  const SparsityPattern& spA = A.get_sparsity_pattern();
  const std::size_t * rowstart = spA.get_rowstart_indices();
  const u_int * colnum = spA.get_column_numbers();

  for(u_int i=0; i< n_dof; ++ i)
  {
	  int bm = sp.dofInfo(i).boundary_mark;
	  std::cout<<"The "<<i<<"-th dof corresponding boundary mark is:"<<bm<<std::endl;
  }

  int numOfbm = 0;
  for (u_int i = 0; i < n_dof; ++ i){
    int bm = sp.dofInfo(i).boundary_mark;
    if (bm == 0) continue;
    // For this case only 1 for Drichlet boundary condition.
    if (bm != 1) continue; // Attention, For this case specially.

    numOfbm += 1;

    for (u_int j = rowstart[i]+1; j < rowstart[i+1]; ++ j){
	    A.global_entry(j) -= A.global_entry(j);
    }
    if (preserve_symmetry) {
      for (u_int j = rowstart[i] + 1;j < rowstart[i + 1];++ j) {
        u_int k = colnum[j];
        const u_int * p = std::find(&colnum[rowstart[k] + 1],
                                    &colnum[rowstart[k + 1]], i);
        if (p != &colnum[rowstart[k+1]]) {
          u_int l = p - &colnum[rowstart[0]];
          A.global_entry(l) -= A.global_entry(l);
        }
      }
    }
  }

  std::cout<<"!@@@@@@@@@@The number of Dirchlet is :"<<numOfbm<<std::endl;

}

int main(int argc, char * argv[])
{
  /// 准备网格
  EasyMesh mesh;
  mesh.readData(argv[1]);

  /// 准备参考单元
  TemplateGeometry<DIM> tmp_geo;
  tmp_geo.readData("triangle.tmp_geo");
  CoordTransform<DIM,DIM> crd_trs;
  crd_trs.readData("triangle.crd_trs");
  TemplateDOF<DIM> tmp_dof(tmp_geo);
  tmp_dof.readData("triangle.2.tmp_dof");
  BasisFunctionAdmin<double,DIM,DIM> bas_fun(tmp_dof);
  bas_fun.readData("triangle.2.bas_fun");

  std::vector<TemplateElement<double,DIM> > tmp_ele(1);
  tmp_ele[0].reinit(tmp_geo, tmp_dof, crd_trs, bas_fun);

  /// 定制有限元空间
  FEMSpace<double,DIM> fem_space(mesh, tmp_ele);
  u_int n_ele = mesh.n_geometry(DIM);
  fem_space.element().resize(n_ele);
  for (u_int i = 0;i < n_ele;++ i) {
    fem_space.element(i).reinit(fem_space, i, 0);
  }
  fem_space.buildElement();
  fem_space.buildDof();
  fem_space.buildDofBoundaryMark();

  /// 准备初值
  FEMFunction<double,DIM> u_h(fem_space);
  Operator::L2Interpolate(&_u_, u_h);

  /// 准备边界条件
  Vector<double> rhs;
  Operator::L2Discretize(&f, fem_space, rhs, 4);

  BoundaryFunction<double,DIM> boundary(BoundaryConditionInfo::DIRICHLET,
                                        1,
                                        &_u_);
  BoundaryConditionAdmin<double,DIM> boundary_admin(fem_space);
  boundary_admin.add(boundary);

  // double t;//

  // double dt = 0.01; /// 简单起见，随手取个时间步长算了

  /// 准备线性系统的矩阵
  /*
  Matrix mat(fem_space, dt);
  mat.algebricAccuracy() = 3;
  mat.build();*/

  StiffMatrix<2,double> stiff_matrix(fem_space);
  stiff_matrix.algebricAccuracy() = 4;
  stiff_matrix.build();
  boundary_condition_apply(fem_space, stiff_matrix, boundary_admin);

  MassMatrix<2,double> mass_matrix(fem_space);
  mass_matrix.algebricAccuracy() = 4;
  mass_matrix.build();
  boundary_condition_apply(fem_space, mass_matrix, boundary_admin);

  /*
  /// 准备右端项
  Vector<double> rhs(fem_space.n_dof());
  FEMSpace<double,DIM>::ElementIterator the_ele = fem_space.beginElement();
  FEMSpace<double,DIM>::ElementIterator end_ele = fem_space.endElement();
  for (;the_ele != end_ele;++ the_ele) {
    double vol = the_ele->templateElement().volume();
    const QuadratureInfo<DIM>& qi = the_ele->findQuadratureInfo(3);
    u_int n_q_pnt = qi.n_quadraturePoint();
    std::vector<double> jac = the_ele->local_to_global_jacobian(qi.quadraturePoint());
    std::vector<AFEPack::Point<DIM>> q_pnt = the_ele->local_to_global(qi.quadraturePoint());
    std::vector<std::vector<double> > bas_val = the_ele->basis_function_value(q_pnt);

    /// 当基函数的值已知情况下，可以使用下面的函数来加速
    std::vector<double> u_h_val = u_h.value(bas_val, *the_ele);
    std::vector<std::vector<double> > u_h_grad = u_h.gradient(q_pnt, *the_ele);
    const std::vector<int>& ele_dof = the_ele->dof();
    u_int n_ele_dof = ele_dof.size();
    for (u_int l = 0;l < n_q_pnt;++ l) {
      double Jxw = vol*qi.weight(l)*jac[l];
      double f_val = _f_(q_pnt[l]);
      for (u_int i = 0;i < n_ele_dof;++ i)
	{
	  rhs(ele_dof[i]) += Jxw*bas_val[i][l]*(u_h_val[l]/dt + f_val);
        }
    }
    }*/

  /// 应用边界条件
  //  boundary_admin.apply(mat, u_h, rhs);

  /// 求解线性系统
  /*
  AMGSolver solver;
  solver.lazyReinit(mat);
  solver.solve(u_h, rhs, 1.0e-08, 50);*/
  //TraceSolver soll(stiff_matrix, mass_matrix);
  CGSolver AAAA;
  EigenSolver solver(stiff_matrix, mass_matrix);
  /*
  std::vector<double>rhs(stiff_matrix.m(),0),x(12,0);
  rhs[0]=-0.107343;
  rhs[1]=-0.107920;
  rhs[2]=0.296721;
  rhs[3]=0.136396;
  rhs[4]=-1.038010;
  rhs[5]=-0.456170;
  rhs[6]=0.694243;
  rhs[7]=1.047650;
  rhs[8]=0.488934;
  rhs[9]=0.065605;
  rhs[10]=-0.113350;
  rhs[11]=-0.309275;
  std::vector<double> temp(6,0);
  std::vector<std::vector<double>> tempX(12,temp);
  solver.X=tempX;
  for(int i=0;i<12;i++)
    {
      for(int j=0;j<6;j++)
	{
	  std::cin>>solver.X[i][j];
	}
    }
  solver.get_MX();
  solver.solve(x, rhs, 1.0e-3, 200);
  std::cout<<"The rhs vector before CG is :\n";
  for(int i=0;i<rhs.size();i++)
    {
      std::cout<<rhs[i]<<" ";
    }
  std::cout<<std::endl;
  std::cout<<"The solution of CG\n";
  for(int i=0;i<x.size();i++)
    {
      std::cout<<x[i]<<" ";
    }
  std::cout<<std::endl;
  */

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

  //solver.mintrace(10, 1.0e-3, 200);
  std::vector<double> x(stiff_matrix.m(),1);
  double lambda;
  solver.PowerSolve(x, lambda);
  /*
  std::cout<<"This is the maximal eigenvalues of AX=lambda Mx;\n";
  for (int i=0;i < x.size();i++)
  {
    std::cout<<x[i]<<" ";
  }
  std::cout<<"\n";
*/
  std::vector<double> x2(stiff_matrix.m(),1);
  solver.IPowerSolve(x2, lambda, 100, 1.e-3);

  int eig_num=5;
  std::vector<double> tempxx(eig_num,0), lam_3(eig_num,0);
  std::vector<std::vector<double>> x3(stiff_matrix.m(),tempxx);
  /*
  for(int k=0;k<eig_num;k++)
  {
    x3[k][k]=1;
  }*/

  for (int i=0;i<x3.size();i++)
  {
    for(int j=0;j<x3[0].size();j++)
    {
      x3[i][j] = ((double) rand() / (RAND_MAX));
    }
  }

  solver.BIPowerSolve(x3, lam_3, eig_num, 1000, 1.e-3);
  std::cout<<"The "<<eig_num<<" smallest eigenvalues are:\n";

  for (int k=0;k<eig_num;k++)
  {
    std::cout<<lam_3[k]<<" ";
  }
  std::cout<<"\n";

  std::cout<<"The matrix are:\n";
  show_matrix(x3);

//  std::cout<<"This is the minimal eigenvalues of AX=lambda Mx;\n";
  /*
  for (int i=0;i < x2.size();i++)
  {
    std::cout<<x2[i]<<" ";
  }
  std::cout<<"\n";
  */
  /*for (int i=0;i<solver.lambda.size();i++)
    {
      std::cout<<solver.lambda[i]<<std::endl;
    }
  std::cout<<"\n";
*/
  /*
  std::vector<double> exact_eigen={2.0*PI*PI,PI*PI,PI*PI,0.0};
  std::cout<<" The exact eigens are:\n";
  for (int i=0;i<solver.lambda.size();i++)
    {
      std::cout<<fabs(solver.lambda[i]-exact_eigen[i])<<std::endl;
      // std::cout<<exact_eigen[i]<<std::endl;
    }
  std::cout<<"\n";

  std::cout<<"This is the matrix V after the whole process\n";
  for(int i=0;i<solver.X.size();i++)
    {
      for(int j=0;j<solver.X[0].size();j++)
	{
	  std::cout<<solver.X[i][j]<<" ";
	}
      std::cout<<std::endl;
      }*/

  /*
  std::cout<<"Output the solution matrix X!\n";
  for(int i=0;i<solver.X.size();i++)
    {
      for(int j=0;j<solver.X[0].size();j++)
	{
	  std::cout<<solver.X[i][j]<<" ";
	}
      std::cout<<std::endl;
    }
  std::cout<<std::endl;
  */

  /// 输出数据画图
  /* u_h.writeOpenDXData("u_h.dx");*/
  // std::cout << "Press ENTER to continue or CTRL+C to stop ..." << std::flush;
  // getchar();

  // t += dt; /// 更新时间

  //std::cout << "\n\tt = " <<  t << std::endl;

  // Print the stiffness matrix in to .txt and .gnuplot form.
 // std::ofstream sparsematrix2 ("stiff_matrix.1");
 // stiff_matrix.print(sparsematrix2);

  std::filebuf fb;
  fb.open ("stiff_matrix.txt",std::ios::out);
  std::ostream os(&fb);
  stiff_matrix.print_formatted(os, 3, true, 0, "0.0", 1);
  fb.close();

 // Print the mass matrix into .txt and .gnuplot form.
//  std::ofstream sparsematrix  ("mass_matrix.1");
 // mass_matrix.print(sparsematrix);

  std::filebuf fb2;
  fb2.open ("mass_matrix.txt",std::ios::out);
  std::ostream os2(&fb2);
  mass_matrix.print_formatted(os2, 3, true, 0, "0.0", 1);
  fb2.close();

 /*
  std::cout<<"Attention! This is print out the columns of the matrix A and M\n";
  std::vector<double> tempx(stiff_matrix.n(),0), tempAx, tempMx;

  for(int i=0;i<solver.A->m();i++)
    {
      tempx[i]=1;

      tempAx=multiply(solver.A, tempx);
      for(int j=0;j<tempAx.size();j++)
	{
	  std::cout<<tempAx[j]<<" ";
	}
	std::cout<<std::endl;

      tempMx=multiply(solver.M, tempx);
      for(int j=0;j<tempMx.size();j++)
	{
	  std::cout<<tempMx[j]<<" ";
	}
      std::cout<<std::endl;
      tempx[i]=0;
    }
    */

  return 0;
}

/**
 * end of file
 *
 */


#include "creapairmatrix.h"
#include <iostream> // for cout
#include <fstream>  // for ifstream
#include <armadillo>
#include <algorithm>    // std::sort

using namespace arma;

// https://stackoverflow.com/questions/11964552/finding-quartiles?utm_medium=organic&utm_source=google_rich_qa&utm_campaign=google_rich_qac++
template <typename T1, typename T2> typename T1::value_type quant(const T1 &x, T2 q)
{
    //assert(q >= 0.0 && q <= 1.0);

    const auto n  = x.size();
    const auto id = (n - 1) * q;
    const auto lo = floor(id);
    const auto hi = ceil(id);
    const auto qs = x[lo];
    const auto h  = (id - lo);

    return (1.0 - h) * qs + h * x[hi];
}

CReadPairMatrix::CReadPairMatrix()
{
    m_alpha = 1.0;
}

int CReadPairMatrix::ReadTSV(const char* filename)
{
    this->matrixValue.empty();
    this->filterdMatrixValue.empty();
    this->nullMatrixValue.empty();
    this->IS2MatrixValue.empty();

    this->vecColName.empty();
    this->vecRowName.empty();

    ifstream infile(filename);
    if (!infile) {
        cout << "unable to load file" << endl;
    }
    string str;

    vector<vector<string>> vvStr;

    vector<string> vecTitle;
    int pos1, pos2;

    // title

    getline(infile, str);
    pos1 = 0;
    pos2 = str.find('\t',pos1);
    pos1 = ++pos2;
    while((pos2 = str.find('\t',pos1))!= string::npos)
    {
        this->vecColName.push_back(str.substr(pos1, pos2-pos1));
        pos1 = ++pos2;
    }
    this->vecColName.push_back(str.substr(pos1, string::npos));   // the last name

    // values
    //int nRowIndex = 0;
    while (getline(infile, str))
    {
        pos1 = 0;
        vector<float> vFloat;
        pos2 = str.find('\t',pos1);
        this->vecRowName.push_back( str.substr(pos1, pos2-pos1) );
        pos1 = ++pos2;
        while((pos2 = str.find('\t',pos1))!= string::npos)
        {
            vFloat.push_back(stof(str.substr(pos1, pos2-pos1)));
            pos1 = ++pos2;
        }
        vFloat.push_back(stof(str.substr(pos1, string::npos)));     // the last value
        //std::sort(vFloat.begin(),vFloat.end());
        //std::cout << nRowIndex << "\t" << quant(vFloat, 0.75) << std::endl;
        //nRowIndex++;
        this->matrixValue.push_back(vFloat);

    }

    float sum = 0;
    for(vector<float> x : this->matrixValue)
        for(float i : x)
            sum += i;

    cout << "Read pair sum = " << sum << endl;

    return 1;
}

int CReadPairMatrix::Nullmatrix()
{

    float TotalSum = 0;
    int nCols = this->vecColName.size();
    int nRows = this->vecRowName.size();

    // total sums
    for(vector<float> x : this->matrixValue)
        for(float i : x)
            TotalSum += i;

    TotalSum += ( nCols * nRows );

    vector<float> colSums;
    // col sums
    for(int i = 0; i < nCols; i++ ){
        float sum = 0;
        for(vector<float> x : this->matrixValue){
            sum += x[i];
            sum += m_alpha;
        }
        colSums.push_back( sum / TotalSum );
    }

    vector<float> rowSums;
    // row sums
    for(vector<float> x : this->matrixValue){
        float sum = 0;
        for(int i = 0; i < nRows; i++ ){
            sum += x[i];
            sum += m_alpha;
        }
        rowSums.push_back( sum / TotalSum );
    }

    nullMatrixValue.empty();
    for(int i = 0; i < nRows; i++ ){
        vector<float> x;
        for(int j = 0; j < nCols; j++ ){
            float nullValue = rowSums[i] * colSums[j];
            x.push_back( nullValue * TotalSum );   //x.push_back( int(nullValue * TotalSum) );
        }
        nullMatrixValue.push_back(x);
    }
}

int CReadPairMatrix::GMM_Fit()
{
    int N = this->vecColName.size() * this->vecRowName.size();

    // create synthetic data containing
    // 2 clusters with normal distribution
    int d = 1; // dimensionality
    //uword N = 10000; // number of samples (vectors)
    mat data(d, N, fill::zeros);

    float sum = 0;
    int i = 0;
    for(vector<float> values : this->matrixValue)
        for(float value : values)
        {
            data.col(i) = value;
            i++;
        }

    // model the data as a diagonal GMM with 2 Gaussians
    gmm_diag model;
    bool status = model.learn(data, 2, maha_dist, random_subset,10, 5, 1e-10, true);
    if(status == false) { cout << "learning failed" << endl; }

    double overall_likelihood = model.avg_log_p(data);

    rowvec set_likelihood = model.log_p( data.cols(0,9) );
    double scalar_likelihood = model.log_p( data.col(0) );

    uword gaus_id = model.assign( data.col(0), eucl_dist );
    urowvec gaus_ids = model.assign( data.cols(0,9), prob_dist );

    filterdMatrixValue.empty();
    // Add filtered values
    i = 0;
    int filtered_cnt = 0;
    int filtered_non_zero_cnt = 0;
    for(vector<float> x : this->matrixValue){
        vector<float> filteredValues;
        for(float value : x){
            uword gaus_id1 = model.assign( data.col(i), eucl_dist ); //prob_dist );
            if ( gaus_id1 == 0) {
                filteredValues.push_back(value);
                cout << "## pos\t" << value << endl;
            }
            else{
                filteredValues.push_back(0.0);
                cout << "## filtered\t" << value << endl;
                filtered_cnt ++;
                if ( value != 0 ) filtered_non_zero_cnt ++;
            }
            i++;
        }
        filterdMatrixValue.push_back(filteredValues);
    }

    urowvec histogram1 = model.raw_hist (data, prob_dist);
    rowvec histogram2 = model.norm_hist(data, eucl_dist);

    model.means.print("means:");

    cout << "filtered_cnt\t" << filtered_cnt << endl;
    cout << "filtered_non_zero_cnt\t" << filtered_non_zero_cnt << endl;
    cout << "histogram1\t" << histogram1[0] << "\t" << histogram1[1] << endl;
    cout << "histogram2\t" << histogram2[0] << "\t" << histogram2[1] << endl;

    model.save("/Users/jyang/my_model.gmm");

    mat modified_dcovs = 2 * model.dcovs;

    model.set_dcovs(modified_dcovs);

    return 0;
}

int CReadPairMatrix::InteractionScore2(vector<vector<float>>& nullMat, vector<vector<float>>& filterdMat)
{
    IS2MatrixValue.empty();

    int nCols = this->vecColName.size();
    int nRows = this->vecRowName.size();

    for(int i = 0; i < nRows; i++ ){
        vector<float> x;
        vector<float> t;
        for(int j = 0; j < nCols; j++ ){
            float nullValue = nullMat[i][j];
            float filteredValue = filterdMat[i][j];
            float IS2 = filteredValue / nullValue;
            x.push_back( IS2 );
            t.push_back( IS2 );
        }

        // Remove the 75th percentile signal
        std::sort(t.begin(),t.end());
        //std::cout << nRowIndex << "\t" << quant(vFloat, 0.75) << std::endl;
        float quantile = quant(t, 0.75);
        if ( quantile == 0.0 ){
            IS2MatrixValue.push_back( x );
        }
        else{
            for(int j = 0; j < nCols; j++ ){
                x[j] -= quantile;
                if ( x[j] < 0.0 ) x[j] = 0.0;
            }
            IS2MatrixValue.push_back( x );
        }

        //std::cout << quant(x, 0.75) << std::endl;
    }
    return 0;
}