#include "../cnum.h"

#ifndef _WINDOWS
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wconversion"
#pragma GCC diagnostic ignored "-Wshadow"
#endif

#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/expint.hpp>
#include <boost/math/special_functions/airy.hpp>

#ifndef _WINDOWS
#pragma GCC diagnostic pop
#endif

using namespace boost::math::policies;

typedef policy<
domain_error    <ignore_error>,
overflow_error  <ignore_error>,
underflow_error <ignore_error>,
denorm_error    <ignore_error>,
pole_error      <ignore_error>,
evaluation_error<ignore_error>,
digits10<8>
> PREC;

//----------------------------------------------------------------------------------------------------------------------
// Bessel functions
//----------------------------------------------------------------------------------------------------------------------

#define BESSEL(NAME, FUNC) \
void NAME(const cnum &n, const cnum &z, cnum &r){ try{\
		r = (!is_real(z) || !is_real(n)) ? UNDEFINED\
		: is_natural(n) ? boost::math::FUNC(to_int(n), z.real(), PREC())\
		: boost::math::FUNC(n.real(), z.real(), PREC());\
	}catch(...){ r = UNDEFINED; }}\
\
double NAME(const cnum &n, const cnum &z){ try{\
		return (!is_real(z) || !is_real(n)) ? UNDEFINED\
		: is_natural(n) ? boost::math::FUNC(to_int(n), z.real(), PREC())\
		: boost::math::FUNC(n.real(), z.real(), PREC());\
	}catch(...){ return UNDEFINED; }}\
\
double NAME(double n, double z){ try{\
		return is_natural(n) ? boost::math::FUNC(to_int(n), z, PREC())\
		: boost::math::FUNC(n, z, PREC());\
	}catch(...){ return UNDEFINED; }}

BESSEL(bessel_J, cyl_bessel_j)
BESSEL(bessel_Y, cyl_neumann)
BESSEL(bessel_I, cyl_bessel_i)
BESSEL(bessel_K, cyl_bessel_k)

#undef BESSEL

//----------------------------------------------------------------------------------------------------------------------
// Elliptic integral Ei
//----------------------------------------------------------------------------------------------------------------------

void expint_i(const cnum &z, cnum &r)
{
	try
	{
		r = is_real(z) ? boost::math::expint(z.real(), PREC()) : UNDEFINED;
	}
	catch(...)
	{
		r = UNDEFINED;
	}
}
double expint_i(const cnum &z)
{
	try
	{
		return is_real(z) ? boost::math::expint(z.real(), PREC()) : UNDEFINED;
	}
	catch(...)
	{
		return UNDEFINED;
	}
}
double expint_i(double z)
{
	try
	{
		return boost::math::expint(z, PREC());
	}
	catch(...)
	{
		return UNDEFINED;
	}
}

//----------------------------------------------------------------------------------------------------------------------
// Elliptic integral En
//----------------------------------------------------------------------------------------------------------------------

void expint_n(const cnum &n, const cnum &z, cnum &r)
{
	try
	{
		r = (is_real(z) && is_natural(n)) ? boost::math::expint(to_natural(n), z.real(), PREC()) : UNDEFINED;
	}
	catch(...)
	{
		r = UNDEFINED;
	}
}
double expint_n(const cnum &n, const cnum &z)
{
	try
	{
		return (is_real(z) && is_natural(n)) ? boost::math::expint(to_natural(n), z.real(), PREC()) : UNDEFINED;
	}
	catch(...)
	{
		return UNDEFINED;
	}
}
double expint_n(double n, double z)
{
	try
	{
		return (is_natural(n)) ? boost::math::expint(to_natural(n), z, PREC()) : UNDEFINED;
	}
	catch(...)
	{
		return UNDEFINED;
	}
}

//----------------------------------------------------------------------------------------------------------------------
// Airy Ai and Bi, Ai' and Bi'
//----------------------------------------------------------------------------------------------------------------------

#define AIRY(NAME, FUNC) \
void NAME(const cnum &z, cnum &r){ try{\
	r = is_real(z) ? boost::math::FUNC(z.real(), PREC()) : UNDEFINED;\
	}catch(...){ r = UNDEFINED; }}\
\
double NAME(const cnum &z){ try{\
	return is_real(z) ? boost::math::FUNC(z.real(), PREC()) : UNDEFINED;\
	}catch(...){ return UNDEFINED; }}\
\
double NAME(double z){ try{\
	return boost::math::FUNC(z, PREC());\
	}catch(...){ return UNDEFINED; }}

AIRY(airy_ai, airy_ai)
AIRY(airy_bi, airy_bi)
AIRY(airy_ai_prime, airy_ai_prime)
AIRY(airy_bi_prime, airy_bi_prime)

#undef AIRY


#include "solver/qp_solver.h"

#include <Eigen/Core>
#include <glog/logging.h>

#include <algorithm>
#include <limits>
#include <string>

#include "math/sparse_cholesky_llt.h"
#include "util/stringprintf.h"

namespace GraphSfM {

QPSolver::QPSolver(const Options& options,
                   const Eigen::SparseMatrix<double>& P,
                   const Eigen::VectorXd& q,
                   const double r)
    : options_(options), P_(P), q_(q), r_(r) {
  CHECK_EQ(P_.rows(), P_.cols()) << "P must be a symmetric matrix.";
  CHECK_EQ(P_.cols(), q_.size())
      << "The dimensions of P and q must be consistent.";

  // Set the lower and upper bounds to be negative and positive infinity (i.e
  // no bounds).
  lb_.setConstant(P_.cols(), -std::numeric_limits<double>::infinity());
  ub_.setConstant(P_.cols(), std::numeric_limits<double>::infinity());

  // Set up the linear solver to compute the cholesky decomposition of:
  //     P_ + rho * eye(N)
  Eigen::SparseMatrix<double> spd_mat(P_.rows(), P_.cols());
  spd_mat.setIdentity();
  spd_mat *= options_.rho;
  spd_mat += P_;

  linear_solver_.Compute(spd_mat);
  CHECK_EQ(linear_solver_.Info(), Eigen::Success);
}

void QPSolver::SetMaxIterations(const int max_iterations) {
  options_.max_num_iterations = max_iterations;
}

void QPSolver::SetUpperBound(const Eigen::VectorXd& ub) { ub_ = ub.array(); }

void QPSolver::SetLowerBound(const Eigen::VectorXd& lb) { lb_ = lb.array(); }

// Solve the quadratic program.
bool QPSolver::Solve(Eigen::VectorXd* solution) {
  // Ensure the bounds are valid. If there are any invalid bounds then the
  // difference between the bounds would be a negative value.
  int coeff_index = -1;
  if ((ub_ - lb_).minCoeff(&coeff_index) < 0) {
    LOG(WARNING) << "You specified invalid lower or upper bounds for the "
                    "problem. lower_bound[" << coeff_index
                 << "] = " << lb_[coeff_index] << " but upper_bound["
                 << coeff_index << "] = " << ub_[coeff_index];
    return false;
  }

  CHECK_NOTNULL(solution)->setZero(q_.size());
  Eigen::VectorXd& x = *solution;
  Eigen::VectorXd z(P_.rows()), u(P_.rows());
  z.setZero();
  u.setZero();

  Eigen::VectorXd z_old(z.size()), x_hat(P_.rows());

  // Precompute some convergence terms.
  const double primal_abs_tolerance_eps =
      std::sqrt(P_.rows()) * options_.absolute_tolerance;
  const double dual_abs_tolerance_eps =
      std::sqrt(P_.rows()) * options_.absolute_tolerance;
  VLOG(2) << "Iteration   Residual         R norm          S norm          "
             "Primal eps      Dual eps";
  const std::string row_format =
      "  % 4d      % 4.4e     % 4.4e     % 4.4e     % 4.4e     % 4.4e";

  // Run the iterations.
  for (int i = 0; i < options_.max_num_iterations; i++) {
    // Update x.
    x.noalias() = linear_solver_.Solve(options_.rho * (z - u) - q_);
    if (linear_solver_.Info() != Eigen::Success) {
      return false;
    }

    // Update x_hat.
    std::swap(z, z_old);
    x_hat.noalias() = options_.alpha * x + (1.0 - options_.alpha) * z_old;

    // Update z.
    z.noalias() = ub_.min(lb_.max((x_hat + u).array())).matrix();

    // Update u.
    u.noalias() += x_hat - z;

    // Compute the convergence terms.
    const double objval = 0.5 * x.dot(P_ * x) + q_.dot(x) + r_;
    const double r_norm = (x - z).norm();
    const double s_norm = (-options_.rho * (z - z_old)).norm();
    const double max_norm = std::max({x.norm(), z.norm()});
    const double primal_eps =
        primal_abs_tolerance_eps + options_.relative_tolerance * max_norm;
    const double dual_eps =
        dual_abs_tolerance_eps +
        options_.relative_tolerance * (options_.rho * u).norm();

    // Log the result to the screen.
    VLOG(2) << StringPrintf(row_format.c_str(), objval, i, r_norm, s_norm,
                            primal_eps, dual_eps);
    // Determine if the minimizer has converged.
    if (r_norm < primal_eps && s_norm < dual_eps) {
      break;
    }
  }
  return true;
}

}  // namespace GraphSfM


#pragma once

#include <Eigen/Dense>
#include <vpp/algorithms/symbols.hh>

namespace vpp
{

  namespace lk_internals
  {
    template <typename F, typename GD>
    auto match(vfloat2 p, vfloat2 tr_prediction,
	       F A, F B, GD Ag,
	       const int winsize,
	       const float min_ev_th,
	       const int max_interations,
	       const float convergence_delta)
    {
      typedef typename F::value_type V;
      int WS = winsize;
      int ws = winsize;
      int hws = ws/2;

      // Gradient matrix
      Eigen::Matrix2f G = Eigen::Matrix2f::Zero();
      int cpt = 0;
      for(int r = -hws; r <= hws; r++)
	for(int c = -hws; c <= hws; c++)
	  {
	    vfloat2 n = p + vfloat2(r, c);
	    if (A.has(n.cast<int>()))
	      {
		Eigen::Matrix2f m;
		auto g = Ag.linear_interpolate(n);
		float gx = g[0];
		float gy = g[1];
		m <<
		  gx * gx, gx * gy,
		  gx * gy, gy * gy;
		G += m;
		cpt++;
	      }
	  }

      // Check minimum eigenvalue.
      float min_ev = 99999.f;
      auto ev = (G / cpt).eigenvalues();
      for (int i = 0; i < ev.size(); i++)
	if (fabs(ev[i].real()) < min_ev) min_ev = fabs(ev[i].real());

      if (min_ev < min_ev_th)
	return std::pair<vfloat2, float>(vfloat2(-1,-1), FLT_MAX);

      Eigen::Matrix2f G1 = G.inverse();

      // Precompute gs and as.
      vfloat2 prediction_ = p + tr_prediction;
      vfloat2 v = prediction_;
      Eigen::Vector2f nk = Eigen::Vector2f::Ones();

      char gs_buffer[WS * WS * sizeof(vfloat2)];
      vfloat2* gs = (vfloat2*) gs_buffer;
      // was: vfloat2 gs[WS * WS];

      typedef plus_promotion<V> S;
      char as_buffer[WS * WS * sizeof(S)];
      S* as = (S*) as_buffer;
      // was: S as[WS * WS];
      {
	for(int i = 0, r = -hws; r <= hws; r++)
	  {
	    for(int c = -hws; c <= hws; c++)
	      {
		vfloat2 n = p + vfloat2(r, c);
		if (Ag.has(n.cast<int>()))
		  {
		    gs[i] = Ag.linear_interpolate(n).template cast<float>();
		    as[i] = cast<S>(A.linear_interpolate(n));
		  }
		i++;
	      }
	  }
      }
      auto domain = B.domain();// - border(hws + 1);

      // Gradient descent
      for (int k = 0; k <= max_interations && nk.norm() >= convergence_delta; k++)
	{
	  Eigen::Vector2f bk = Eigen::Vector2f::Zero();
	  // Temporal difference.
	  int i = 0;
	  for(int r = -hws; r <= hws; r++)
	    {
	      for(int c = -hws; c <= hws; c++)
		{
		  vfloat2 n = p + vfloat2(r, c);
		  if (Ag.has(n.cast<int>()))
		    {
		      vfloat2 n2 = v + vfloat2(r, c);
		      auto g = gs[i];
		      float dt = (cast<float>(as[i]) - cast<float>(B.linear_interpolate(n2)));
		      bk += Eigen::Vector2f{g[0] * dt, g[1] * dt};
		    }
		  i++;
		}
	    }

	  nk = G1 * bk;
	  v += vfloat2{nk[0], nk[1]};

	  if (!domain.has(v.cast<int>()))
	    return std::pair<vfloat2, float>(vfloat2(0, 0), FLT_MAX);
	}

      // Compute the SSD.
      float err = 0;
      for(int r = -hws; r <= hws; r++)
	for(int c = -hws; c <= hws; c++)
	  {
	    vfloat2 n2 = v + vfloat2(r, c);
	    int i = (r+hws) * ws + (c+hws);
	    {
	      err += fabs(cast<float>(as[i] - cast<S>(B.linear_interpolate(n2))));
	      cpt++;
	    }
	  }

      return std::pair<vfloat2, float>(v - p, err / (cpt));

    }
  }

  template <typename V, typename... OPTS>
  void lucas_kanade(const image2d<V>& i1,
		    const image2d<V>& i2,
		    OPTS... opts)
  {
    auto options = iod::D(opts...);
    int niterations = options.get(_niterations, 21);
    int winsize = options.get(_winsize, 11);
    int nscales = options.get(_nscales, 3);
    int min_ev = options.get(_min_ev, 0.0001);
    int delta = options.get(_delta, 0.1);
    auto prediction = options.get(_prediction,
				  [] (auto p) { return vfloat2(0.f, 0.f); });
    auto flow = options.flow;

    auto keypoints = options.keypoints;

    typedef std::decay_t<decltype(V() - V())> Gr;
    pyramid2d<V> pyramid_prev(i1, nscales, 2, _border = winsize / 2);
    pyramid2d<vector<Gr, 2>> pyramid_prev_grad(i1.domain(), nscales, 2, _border = winsize / 2);
    pyramid2d<V> pyramid_next(i2, nscales, 2, _border = winsize / 2);

    scharr(pyramid_prev[0], pyramid_prev_grad[0]);
    pyramid_prev_grad.propagate_level0();

    for (int i = 0; i < keypoints.size(); i++)
    {
      auto kp = keypoints[i];

      vfloat2 tr = (prediction(kp)).template cast<float>() / float(std::pow(2, nscales));
      float dist = 0.f;
      for(int S = pyramid_prev.size() - 1; S >= 0; S--)
	{
	  tr *= pyramid_prev.factor();
	  auto match = lk_internals::match(kp.template cast<float>() / int(std::pow(2, S)),
					   tr,
					   pyramid_prev[S],
					   pyramid_next[S],
					   pyramid_prev_grad[S],
					   winsize,
					   min_ev,
					   niterations, delta);

	  tr = match.first;
	  dist = match.second;
	}

      flow(kp, tr, dist);

    }
  }

}


#ifndef LA_TOOLS_HPP_
#define LA_TOOLS_HPP_

#include <algorithm> //min
#include <iostream>

#include <NTL/mat_GF2.h>

#include <xtensor/xarray.hpp>
#include <xtensor/xview.hpp>
#include <xtensor-blas/xlinalg.hpp>

void gf2_syndrome(xt::xarray<int> *s, xt::xarray<int> *y,xt::xarray<int> *H);
xt::xarray<int> gf2_syndrome(xt::xarray<int> *y,xt::xarray<int> *H);

void gf2_rank(int* Matrix, int n_c, int n_v, int* r);
int gf2_rank(int* Matrix, int n_c, int n_v);

bool gf2_isEquiv(xt::xarray<int> e, xt::xarray<int> H, int n_c, int n_v);

void gf4_syndrome(xt::xarray<int> *s, xt::xarray<int> *y,xt::xarray<int> *H);
xt::xarray<int> gf4_syndrome(xt::xarray<int> *y,xt::xarray<int> *H);

void gf4_rank(int* Matrix, int n_c, int n_v, int* r);
int gf4_rank(int* Matrix, int n_c, int n_v);

bool gf4_isEquiv(xt::xarray<int> e, xt::xarray<int> H, int n_c, int n_v);

int gf4_mul(int a, int b);
int gf4_conj(int a);

int hamming_weight(xt::xarray<int>x);

xt::xarray<int> get_x(xt::xarray<int> y);
xt::xarray<int> get_z(xt::xarray<int> y);

#endif


#include <math.h>
#include <EigenUnsupported/Eigen/KroneckerProduct>
#include "Core/Utilities/QProgInfo/QCircuitInfo.h"
#include "Core/Utilities/Tools/MatrixDecomposition.h"
#include <chrono>
#include "Core/Utilities/QProgInfo/Visualization/QVisualization.h"
#include "QAlg/Base_QCircuit/AmplitudeEncode.h"

USING_QPANDA
using namespace std;
using namespace chrono;

#define PRINT_TRACE 0
#if PRINT_TRACE
#define PTrace printf
#define PTraceMat(mat) (std::cout << (mat) << endl)
#define PTraceCircuit(cir) (std::cout << cir << endl)
#else
#define PTrace
#define PTraceMat(mat)
#define PTraceCircuit(cir)
#endif

#define MAX_MATRIX_PRECISION 1e-10

using MatrixSequence = std::vector<MatrixUnit>;
using DecomposeEntry = std::pair<int, MatrixSequence>;

using ColumnOperator = std::vector<DecomposeEntry>;
using MatrixOperator = std::vector<ColumnOperator>;

using SingleGateUnit = std::pair<MatrixSequence, QStat>;

static void upper_partition(int order, MatrixOperator& entries)
{
	auto index = (int)std::log2(entries.size() + 1) - (int)std::log2(order) - 1;

	for (auto cdx = 0; cdx < order - 1; ++cdx)
	{
		for (auto rdx = 0; rdx < order - cdx - 1; ++rdx)
		{
			auto entry = entries[cdx][rdx];

			entry.first += order;
			entry.second[index] = MatrixUnit::SINGLE_P1;

			entries[cdx + order].emplace_back(entry);
		}
	}

	return;
}

static bool entry_requirement(const MatrixSequence& units, int udx, int cdx)
{
	int lj = ((cdx - 1) >> (udx - 1)) & 1;

	int M = 1;
	while (cdx)
	{
		cdx >>= 1;
		M += cdx ? 1 : 0;
	}

	//if 1 ≤ j ≤ m and cj = lj' = 1 , return true
	auto mat = units[units.size() - udx];
	return udx >= 1
		&& udx <= M
		&& lj
		&& mat == MatrixUnit::SINGLE_P1;
}

static bool steps_requirement(const MatrixSequence& units, int udx, int cdx)
{
	int M = 1;
	while (cdx)
	{
		cdx >>= 1;
		M += cdx ? 1 : 0;
	}

	//if j = n and none of cn...cm+1 is 1 , return true
	if (units.size() != udx)
	{
		return false;
	}
	else
	{
		auto iter = std::find(units.begin(), units.end() - M, MatrixUnit::SINGLE_P1);
		return (units.end() - M) == iter;
	}
}

static void under_partition(int order, MatrixOperator& entries)
{
	auto qubits = (int)std::log2(entries.size() + 1);

	for (auto cdx = 1; cdx < order; ++cdx)
	{
		if (cdx & 1)
		{
			for (auto rdx = 0; rdx < order; ++rdx)
			{
				auto value = entries[0][rdx + order - 1].first ^ cdx;
				auto entry = make_pair(value, entries[cdx - 1][rdx + order - cdx].second);

				entries[cdx].emplace_back(entry);
			}

			auto& units = entries[cdx].back().second;
			for (auto idx = 0; idx < (int)std::log2(order); ++idx)
			{
				units[qubits - idx - 1] = ((cdx >> idx) & 1) ?
					MatrixUnit::SINGLE_P1 : MatrixUnit::SINGLE_I2;
			}
		}
		else
		{
			for (auto rdx = 0; rdx < order; ++rdx)
			{
				auto range = (int)std::log2(order) + 1;
				auto refer = entries[0][rdx + order - 1].second;
				auto entry = entries[0][rdx + order - 1].first ^ cdx;

				MatrixSequence units(refer.begin() + qubits - range, refer.end());

				for (auto udx = 1; udx <= range; ++udx)  /*udx = j , cdx = L*/
				{
					bool steps_accord = steps_requirement(units, udx, cdx + 1);
					bool entry_accord = entry_requirement(units, udx, cdx + 1);

					units[range - udx] = steps_accord ? MatrixUnit::SINGLE_P1 :
						entry_accord ? MatrixUnit::SINGLE_P0 : units[range - udx];
				}

				for (auto idx = 0; idx < qubits - range; ++idx)
				{
					units.insert(units.begin(), MatrixUnit::SINGLE_I2);
				}

				entries[cdx].emplace_back(make_pair(entry, units));
			}

			auto refer_opt = entries[0][2 * order - 2].second;
			for (auto idx = 0; idx < qubits; ++idx)
			{
				if ((cdx >> idx) & 1)
				{
					refer_opt[qubits - idx - 1] = MatrixUnit::SINGLE_P1;
				}
			}

			entries[cdx].back().second = refer_opt;
		}
	}

	return;
}

static void controller(MatrixSequence& sequence, const EigenMatrix2c U2, EigenMatrixXc& matrix)
{
	EigenMatrix2c P0;
	EigenMatrix2c P1;
	EigenMatrix2c I2;

	P0 << Eigen::dcomplex(1, 0), Eigen::dcomplex(0, 0),
		Eigen::dcomplex(0, 0), Eigen::dcomplex(0, 0);
	P1 << Eigen::dcomplex(0, 0), Eigen::dcomplex(0, 0),
		Eigen::dcomplex(0, 0), Eigen::dcomplex(1, 0);
	I2 << Eigen::dcomplex(1, 0), Eigen::dcomplex(0, 0),
		Eigen::dcomplex(0, 0), Eigen::dcomplex(1, 0);

	std::map<MatrixUnit, std::function<EigenMatrix2c()>> mapping =
	{
		{ MatrixUnit::SINGLE_P0, [&]() {return P0; } },
		{ MatrixUnit::SINGLE_P1, [&]() {return P1; } },
		{ MatrixUnit::SINGLE_I2, [&]() {return I2; } },
		{ MatrixUnit::SINGLE_V2, [&]() {return U2 - I2; } }
	};

	auto order = sequence.size();
	EigenMatrixXc Un = EigenMatrixXc::Identity(1, 1);
	EigenMatrixXc In = EigenMatrixXc::Identity(1ull << order, 1ull << order);

	for (const auto& val : sequence)
	{
		EigenMatrix2c M2 = mapping.find(val)->second();
		Un = Eigen::kroneckerProduct(Un, M2).eval();
	}

	matrix = In + Un;
	return;
}

static void recursive_partition(const EigenMatrixXc& sub_matrix, MatrixOperator& entries)
{
	Eigen::Index order = sub_matrix.rows();
	if (1 == order)
	{
		return;
	}
	else
	{
		EigenMatrixXc corner = sub_matrix.topLeftCorner(order / 2, order / 2);

		recursive_partition(corner, entries);

		upper_partition(order / 2, entries);
		under_partition(order / 2, entries);
	}

	return;
}

static void decomposition(EigenMatrixXc& matrix, MatrixOperator& entries, std::vector<SingleGateUnit>& cir_units)
{
	for (auto cdx = 0; cdx < entries.size(); ++cdx)
	{
		auto opts = entries[cdx].size();
		for (auto idx = 0; idx < opts; ++idx)
		{
			auto rdx = entries[cdx][idx].first;
			auto opt = entries[cdx][idx].second;

			if ((EigenComplexT(0, 0) == matrix(rdx, cdx) && (idx != opts - 1)) ||
				(EigenComplexT(1, 0) == matrix(cdx, cdx) && (idx == opts - 1)))
			{
				continue;
			}
			else
			{
				EigenMatrix2c C2; /*placeholder*/
				C2 << EigenComplexT(0, 1), EigenComplexT(0, 1),
					EigenComplexT(0, 1), EigenComplexT(0, 1);

				EigenMatrixXc Cn;
				controller(opt, C2, Cn);

				Qnum indices(2);
				for (Eigen::Index index = 0; index < (1ull << opt.size()); ++index)
				{
					if (Cn(rdx, index) != EigenComplexT(0, 0))
					{
						indices[index == rdx] = index;
					}
				}

				EigenComplexT C0 = matrix(indices[0], cdx);  /*The entry to be eliminated */
				EigenComplexT C1 = matrix(indices[1], cdx);  /*The corresponding entry */

				EigenComplexT V11, V12, V21, V22;

				if (indices[0] < indices[1])
				{
					V11 = std::conj(C0) / std::sqrt(std::norm(C0) + std::norm(C1));
					V12 = std::conj(C1) / std::sqrt(std::norm(C0) + std::norm(C1));
					V21 = C1 / std::sqrt(std::norm(C0) + std::norm(C1));
					V22 = -C0 / std::sqrt(std::norm(C0) + std::norm(C1));
				}
				else
				{
					V11 = -C0 / std::sqrt(std::norm(C0) + std::norm(C1));
					V12 = C1 / std::sqrt(std::norm(C0) + std::norm(C1));
					V21 = std::conj(C1) / std::sqrt(std::norm(C0) + std::norm(C1));
					V22 = std::conj(C0) / std::sqrt(std::norm(C0) + std::norm(C1));
				}

				EigenMatrix2c V2;
				V2 << V11, V12, V21, V22;

				EigenMatrixXc Un;
				controller(opt, V2, Un);

				matrix = Un * matrix;

				QStat M2 = { (qcomplex_t)V11 ,(qcomplex_t)V12 ,(qcomplex_t)V21 ,(qcomplex_t)V22 };
				cir_units.insert(cir_units.begin(), std::make_pair(opt, M2));
			}
		}
	}

	EigenMatrix2c V2 = matrix.bottomRightCorner(2, 2);
	if (EigenMatrixXc::Identity(2, 2) != V2)
	{
		QPANDA_ASSERT((V2(0, 0) * V2(1, 1)) == (V2(0, 1) * V2(1, 0)), "decomposition error on matrix.bottomRightCorner(2, 2)");

		qcomplex_t E0 = V2(1, 1) / ((V2(0, 0) * V2(1, 1)) - (V2(0, 1) * V2(1, 0)));
		qcomplex_t E1 = V2(0, 1) / ((V2(0, 1) * V2(1, 0)) - (V2(0, 0) * V2(1, 1)));
		qcomplex_t E2 = V2(1, 0) / ((V2(0, 1) * V2(1, 0)) - (V2(0, 0) * V2(1, 1)));
		qcomplex_t E3 = V2(0, 0) / ((V2(0, 0) * V2(1, 1)) - (V2(0, 1) * V2(1, 0)));

		QStat M2 = { E0 ,E1 ,E2 ,E3 };

		auto entry = entries.back().back().second;
		cir_units.insert(cir_units.begin(), std::make_pair(entry, M2));
	}
}

static void initialize(EigenMatrixXc& matrix, MatrixOperator& entries)
{
	auto qubits = (int)std::log2(matrix.rows());

	MatrixSequence Cns(qubits, MatrixUnit::SINGLE_I2);
	Cns.back() = MatrixUnit::SINGLE_V2;
	entries.front().emplace_back(make_pair(1, Cns));

	ColumnOperator& column = entries.front();
	for (auto idx = 1; idx < qubits; ++idx)
	{
		size_t path = 1ull << idx;
		for (auto opt = 0; opt < (1 << idx) - 1; ++opt)
		{
			auto entry = column[opt].first;
			auto units = column[opt].second;

			// 1 : none of cn−1, . . . , c1 equals 1
			// * : otherwise
			auto iter = std::find(units.end() - idx, units.end(), MatrixUnit::SINGLE_P1);

			units[units.size() - 1 - idx] = (units.end() == iter) ?
				MatrixUnit::SINGLE_P1 : MatrixUnit::SINGLE_I2;

			column.emplace_back(make_pair(entry + path, units));
		}

		MatrixSequence Lns(qubits, MatrixUnit::SINGLE_I2);
		Lns[qubits - idx - 1] = MatrixUnit::SINGLE_V2;

		column.emplace_back(make_pair((1ull << idx), Lns));
	}

	return;
}

static void general_scheme(EigenMatrixXc& matrix, std::vector<SingleGateUnit>& cir_units)
{
	MatrixOperator entries;
	for (auto idx = 1; idx < matrix.cols(); ++idx)
	{
		ColumnOperator Co;
		entries.emplace_back(Co);
	}

	initialize(matrix, entries);
	recursive_partition(matrix, entries);
	decomposition(matrix, entries, cir_units);

	return;
}

static void circuit_insert(QVec& qubits, std::vector<SingleGateUnit>& cir_units, QCircuit& circuit, bool b_positive_seq)
{
	if (b_positive_seq)
	{
		std::sort(qubits.begin(), qubits.end(), [&](Qubit* a, Qubit* b) {
			return a->getPhysicalQubitPtr()->getQubitAddr()
				< b->getPhysicalQubitPtr()->getQubitAddr();
			});
	}
	else
	{
		std::sort(qubits.begin(), qubits.end(), [&](Qubit* a, Qubit* b) {
			return a->getPhysicalQubitPtr()->getQubitAddr()
				> b->getPhysicalQubitPtr()->getQubitAddr();
			});
	}

	auto rank = qubits.size();
	for (auto& val : cir_units)
	{
		QVec control;
		QCircuit cir;

		for (auto qdx = 0; qdx < rank; qdx++)
		{
			if (MatrixUnit::SINGLE_P0 == val.first[qdx])
			{
				cir << X(qubits[qdx]);
				control.emplace_back(qubits[qdx]);
			}
			else if (MatrixUnit::SINGLE_P1 == val.first[qdx])
			{
				control.emplace_back(qubits[qdx]);
			}
			else
			{
			}
		}

		for (auto qdx = 0; qdx < rank; qdx++)
		{
			if (MatrixUnit::SINGLE_V2 == val.first[qdx])
			{
				circuit << cir
					<< U4(val.second, qubits[qdx]).control(control).dagger()
					<< cir;

				break;
			}
		}
	}
}

/*******************************************************************
*                      class DiagonalMatrixDecompose
********************************************************************/
class DiagonalMatrixDecompose
{
public:
	DiagonalMatrixDecompose() {}
	~DiagonalMatrixDecompose() {}

	QCircuit decompose(const QVec& qubits, const QStat& src_mat)
	{
		//check param
		if (!is_unitary_matrix(src_mat))
		{
			QCERR_AND_THROW_ERRSTR(run_fail, "Error: failed on HQRDecompose, the input matrix is not a unitary-matrix.");
		}

		const auto mat_dimension = sqrt(src_mat.size());
		const auto need_qubits_num = ceil(log2(mat_dimension));
		if (need_qubits_num > qubits.size())
		{
			QCERR_AND_THROW_ERRSTR(run_fail, "Error: failed on HQRDecompose, no enough qubits.");
		}

		QCircuit decompose_result_cir;
		m_qubits = qubits;

		QVec controlqvec = qubits;
		controlqvec.pop_back();
		QStat tmp_mat22; //2*2 unitary matrix
		const size_t tmp_base_unitary_cnt = mat_dimension / 2;
		long pre_index = -1;
		for (size_t i = 0; i < tmp_base_unitary_cnt; ++i)
		{
			if (0 == i)
			{
				QCircuit index_cir_zero = index_to_circuit(0, controlqvec);
				decompose_result_cir << index_cir_zero;
			}
			else
			{
				QCircuit index_cir = index_to_merge_circuit(i, pre_index, controlqvec);
				decompose_result_cir << index_cir;
			}

			tmp_mat22.clear();
			const size_t tmp_row = (2 * i * mat_dimension) + (2 * i);
			tmp_mat22.push_back(src_mat[tmp_row]);
			tmp_mat22.push_back(src_mat[tmp_row + 1]);
			tmp_mat22.push_back(src_mat[tmp_row + mat_dimension]);
			tmp_mat22.push_back(src_mat[tmp_row + mat_dimension + 1]);
			QGate tmp_u4 = U4(tmp_mat22, qubits.back()).control(controlqvec);
			QGATE_SPACE::U4* p_gate = dynamic_cast<QGATE_SPACE::U4*>(tmp_u4.getQGate());
			if ((abs(p_gate->getAlpha()) > MAX_MATRIX_PRECISION)
				|| (abs(p_gate->getBeta()) > MAX_MATRIX_PRECISION)
				|| (abs(p_gate->getGamma()) > MAX_MATRIX_PRECISION)
				|| (abs(p_gate->getDelta()) > MAX_MATRIX_PRECISION))
			{
				decompose_result_cir << tmp_u4;
			}

			pre_index = i;
		}

		return decompose_result_cir;
	}

protected:
	QCircuit index_to_circuit(size_t index, QVec& controlqvec)
	{
		QCircuit ret_cir;
		size_t data_qubits_cnt = controlqvec.size();
		for (size_t i = 0; i < data_qubits_cnt; ++i)
		{
			if (0 == index % 2)
			{
				ret_cir << X(controlqvec[data_qubits_cnt - i - 1]);
			}

			index /= 2;
		}

		return ret_cir;
	}

	QCircuit index_to_merge_circuit(size_t index, QVec& controlqvec)
	{
		if (0 == index)
		{
			QCERR_AND_THROW_ERRSTR(run_fail, "Error: failed to build merge-index-circuit, the index must be >0.");
		}

		size_t pre_index = index - 1;
		QCircuit ret_cir;
		size_t data_qubits_cnt = controlqvec.size();
		for (size_t i = 0; i < data_qubits_cnt; ++i)
		{
			if ((index % 2) != (pre_index % 2))
			{
				ret_cir << X(controlqvec[data_qubits_cnt - i - 1]);
			}

			index /= 2;
			pre_index /= 2;
		}

		return ret_cir;
	}

	QCircuit index_to_merge_circuit(size_t index, long pre_index, QVec& controlqvec)
	{
		if (0 == index)
		{
			QCERR_AND_THROW_ERRSTR(run_fail, "Error: failed to build merge-index-circuit, the index must be >0.");
		}

		size_t tmp_pre_index = pre_index;
		if (pre_index < 0)
		{
			tmp_pre_index = 1;
		}

		QCircuit ret_cir;
		size_t data_qubits_cnt = controlqvec.size();
		for (size_t i = 0; i < data_qubits_cnt; ++i)
		{
			if ((index % 2) != (tmp_pre_index % 2))
			{
				ret_cir << X(controlqvec[data_qubits_cnt - i - 1]);
			}

			index /= 2;

			if (pre_index > 0)
			{
				tmp_pre_index /= 2;
			}
		}

		return ret_cir;
	}

private:
	QVec m_qubits;
};

/*******************************************************************
*                      public interface
********************************************************************/
QCircuit QPanda::matrix_decompose_qr(QVec qubits, const QStat& src_mat, const bool b_positive_seq)
{
	auto order = std::sqrt(src_mat.size());
	EigenMatrixXc tmp_mat = EigenMatrixXc::Map(&src_mat[0], order, order);

	return matrix_decompose_qr(qubits, tmp_mat, b_positive_seq);
}

QCircuit QPanda::matrix_decompose_qr(QVec qubits, EigenMatrixXc& src_mat, const bool b_positive_seq)
{
	if (!src_mat.isUnitary(MAX_MATRIX_PRECISION))
	{
		QCERR_AND_THROW_ERRSTR(invalid_argument, "Non-unitary matrix.");
	}

	if (qubits.size() != log2(src_mat.cols()))
	{
		QCERR_AND_THROW_ERRSTR(invalid_argument, "The qubits number is error or the input matrix is not a 2^n-dimensional matrix.");
	}

	QCircuit output_circuit;
	//QR decompose
	std::vector<SingleGateUnit> cir_units;
	general_scheme(src_mat, cir_units);
	circuit_insert(qubits, cir_units, output_circuit, b_positive_seq);

	return output_circuit;
}

QCircuit QPanda::diagonal_matrix_decompose(const QVec& qubits, const QStat& src_mat)
{
	return DiagonalMatrixDecompose().decompose(qubits, src_mat);
}

/*******************************************************************
*                    puali-XYZ  decomposition
********************************************************************/

MatrixToPauli::MatrixToPauli(QuantumMachine* qvm)
{
	m_qvm = qvm;
}

MatrixToPauli::~MatrixToPauli()
{
	//destroyQuantumMachine(m_qvm);
}

void MatrixToPauli::matrixDecompositionNew(QMatrix<double>& qmat)
{
	int size = qmat.size;
	int numbits = ceil(log2(size));
	auto a = qAllocMany(numbits);
	unsigned short index;
	for (int i = 0; i < size - 1; i++) {
		for (int j = i + 1; j < size; j++) {
			if (qmat(i, j) != 0 && qmat(j, i) != 0) {
				index = 1;
			}
			else if (qmat(i, j) != 0 && qmat(j, i) == 0) {
				index = 2;
			}
			else if (qmat(i, j) == 0 && qmat(j, i) != 0) {
				index = 3;
			}
			else {
				index = 10;
			}
			matrixDecompositionSub(qmat.data, i, j, index, numbits, a);
		}
	}
	add2CirAndCoeII(qmat.data, a);
}

std::vector<int> MatrixToPauli::ASCII2BIN(int a)
{
	int d = a;
	vector<int> binary;
	if (d == 0)
	{
		binary.push_back(0);
	}
	else
	{
		while (d != 0)
		{
			binary.push_back(d % 2);
			d /= 2;
		}
	}
	return binary;
}

void MatrixToPauli::add2CirAndCoeIJ(std::vector<double>& mat, int i, int j, const QVec& a)
{
	auto BinIndex = convert2FullBinaryIndex(a.size(), i, j);
	auto pauliCircuitIJ = convert2PauliOperator(BinIndex.first, BinIndex.second, a);
	auto pauliCircuitJI = convert2PauliOperator(BinIndex.second, BinIndex.first, a);
	auto signIJ = pauliCircuitIJ.second;
	auto signJI = pauliCircuitJI.second;
	int num = signIJ.size();
	vector<double> Mcoe(num);
	int size = 1 << a.size();
	for (int K = 0; K < num; K++)
	{
		Mcoe[K] = mat[i * size + j] * signIJ[K] + mat[j * size + i] * signJI[K];
	}
	addCoeAndCirAtMij(1, pauliCircuitIJ.first, Mcoe);
}

void MatrixToPauli::matrixDecompositionSub(std::vector<double>& mat,
	int i,
	int j,
	unsigned short index,
	int numbits,
	const QVec& a)
{
	switch (index)
	{
	case 1:
		add2CirAndCoeIJ(mat, i, j, a);
		break;
	case 2:
		add2CirAndCoeIorJ(mat, i, j, a);
		break;
	case 3:
		add2CirAndCoeIorJ(mat, j, i, a);
		break;
	case 10:
		break;
	}
}

void MatrixToPauli::add2CirAndCoeIorJ(std::vector<double>& mat, int i, int j, const QVec& a)
{
	auto BinIndex = convert2FullBinaryIndex(a.size(), i, j);
	auto pauliCircuitIJ = convert2PauliOperator(BinIndex.first, BinIndex.second, a);
	int size = 1 << a.size();
	addCoeAndCirAtMij(mat[i * size + j], pauliCircuitIJ.first, pauliCircuitIJ.second);
}

void MatrixToPauli::add2CirAndCoeII(std::vector<double>& mat, const QVec& a)
{
	vector<vector<int>> signs;
	vector<QCircuit> PauliCirDiag;
	int size = 1 << a.size();
	auto BinIndex = convert2FullBinaryIndex(a.size(), 0, 0);
	auto pauliCircuitII = convert2PauliOperator(BinIndex.first, BinIndex.second, a);
	PauliCirDiag = pauliCircuitII.first;
	signs.push_back(pauliCircuitII.second);
	for (int i = 1; i < size; i++)
	{
		auto BinIndex = convert2FullBinaryIndex(a.size(), i, i);
		auto pauliCoefficient = convert2Coefficient(BinIndex.first, BinIndex.second);
		signs.push_back(pauliCoefficient);
	}
	vector<double> Mcoe(signs.size());
	for (int j = 0; j < signs.size(); j++)
	{
		double sum = 0;
		for (int i = 0; i < size; i++)
		{
			sum = mat[i * size + i] * signs[i][j] + sum;
		}
		Mcoe[j] = sum;
	}
	addCoeAndCirAtMij(1, pauliCircuitII.first, Mcoe);
}

std::pair<std::vector<QCircuit>, std::vector<int>> MatrixToPauli::convert2PauliOperator
                                        (const std::vector<int>& i_s,
	                                     const std::vector<int>& j_s,
	                                     const QVec& a)
{
	int num = i_s.size();
	std::vector<QCircuit> Pauli_i(num);
	std::vector<QCircuit> Pauli_j(num);
	std::vector<QCircuit> Pauli((1 << num));
	std::vector<int> sign;
	std::vector<int> signFull((1 << num));
	for (int i = 0; i < num; i++)
	{
		if (i_s[i] == 0 && j_s[i] == 0)
		{
			Pauli_i[i] << I(a[num - i - 1]);
			Pauli_j[i] << Z(a[num - i - 1]);
			sign.push_back(1);
		}
		if (i_s[i] == 1 && j_s[i] == 1)
		{
			Pauli_i[i] << I(a[num - i - 1]);
			Pauli_j[i] << Z(a[num - i - 1]);
			sign.push_back(-1);
		}
		if (i_s[i] == 0 && j_s[i] == 1)
		{
			Pauli_i[i] << X(a[num - i - 1]);
			Pauli_j[i] << Z(a[num - i - 1]);
			Pauli_j[i] << X(a[num - i - 1]);
			sign.push_back(1);
		}
		if (i_s[i] == 1 && j_s[i] == 0) {
			Pauli_i[i] << X(a[num - i - 1]);
			Pauli_j[i] << Z(a[num - i - 1]);
			Pauli_j[i] << X(a[num - i - 1]);
			sign.push_back(-1);
		}
	}
	for (int i = 1; i < ((1 << num) + 1); i++)
	{
		int k = i;
		int count = 0;
		signFull[i - 1] = 1;
		while (count < num) {
			if (k <= (1 << (num - count - 1)))
			{
				Pauli[i - 1] << Pauli_i[count];
				signFull[i - 1] = signFull[i - 1] * 1;
			}
			else
			{
				Pauli[i - 1] << Pauli_j[count];
				k = k - (1 << (num - count - 1));
				signFull[i - 1] = signFull[i - 1] * sign[count];
			}
			count++;
		}
	}
	return make_pair(Pauli, signFull);
}

std::vector<int> MatrixToPauli::convert2Coefficient(const std::vector<int>& i_s,
	                                                const std::vector<int>& j_s)
{
	int num = i_s.size();
	vector<int> sign;
	vector<int> signFull((1 << num));
	for (int i = 0; i < num; i++)
	{
		if (i_s[i] == 0 && j_s[i] == 0)
		{
			sign.push_back(1);
		}
		if (i_s[i] == 1 && j_s[i] == 1)
		{
			sign.push_back(-1);
		}
		if (i_s[i] == 0 && j_s[i] == 1)
		{
			sign.push_back(1);
		}
		if (i_s[i] == 1 && j_s[i] == 0)
		{
			sign.push_back(-1);
		}
	}
	for (int i = 1; i < ((1 << num) + 1); i++)
	{
		int k = i;
		int count = 0;
		signFull[i - 1] = 1;
		while (count < num) {
			if (k <= (1 << num - count - 1))
			{
				signFull[i - 1] = signFull[i - 1] * 1;
			}
			else
			{
				k = k - (1 << num - count - 1);
				signFull[i - 1] = signFull[i - 1] * sign[count];
			}
			count++;
		}
	}
	return signFull;
}

template <typename V>
void MatrixToPauli::addCoeAndCirAtMij(double ma, const std::vector<QCircuit>& cir, V& sign)
{
	int num = sign.size();
	for (int i = 0; i < num; i++)
	{
		if (ma * sign[i] != 0)
		{
			m_QMcoe.push_back(ma * sign[i] / num);
			m_QMcir.push_back(cir[i]);
		}
	}
}

std::pair<std::vector<int>, std::vector<int>> MatrixToPauli::convert2FullBinaryIndex(int numbits,
	unsigned long i,
	unsigned long j)
{
	auto rowi = ASCII2BIN(i);
	auto colj = ASCII2BIN(j);
	if (rowi.size() < numbits)
	{
		for (int k = rowi.size(); k < numbits; ++k)
			rowi.push_back(0);
	}
	if (colj.size() < numbits)
	{
		for (int k = colj.size(); k < numbits; ++k)
			colj.push_back(0);
	}
	reverse(rowi.begin(), rowi.end());
	reverse(colj.begin(), colj.end());
	return make_pair(rowi, colj);
}

void MatrixToPauli::combine_same_circuit()
{
	int size = m_QMcir.size();
	int num = ceil(log2(size));
	std::vector<int> repeatedindex;
	for (int i = 0; i < size; i++)
	{
		if (!matchIndex(i, repeatedindex))
		{
			std::vector<int> index;
			for (int j = i + 1; j < size; j++)
			{
				if (matchTwoCircuit(m_QMcir[i], m_QMcir[j]))
				{
					index.push_back(j);
					repeatedindex.push_back(j);
				}
			}
			addtoSimplyCircuit(i, index, num);
			index.clear();
		}
	}
}

bool MatrixToPauli::matchIndex(int k, const std::vector<int>& repeatedindex)
{
	int size = repeatedindex.size();
	bool matched = false;
	if (size == 0)
	{
		matched = false;
	}
	else
	{
		for (int i = 0; i < size; i++)
		{
			if (k == repeatedindex[i])
			{
				matched = true;
				break;
			}
		}
	}
	return matched;
}

bool MatrixToPauli::matchTwoCircuit(const QCircuit& a, const QCircuit& b, bool criteria_matrix_circuit)
{
	bool matched = false;
	if (!criteria_matrix_circuit)
	{
		QuantumMachine* mach = initQuantumMachine(CPU);
		auto prog_a = createEmptyQProg();
		auto prog_b = createEmptyQProg();
		prog_a << a;
		prog_b << b;
		auto instrcution_a = transformQProgToOriginIR(prog_a, mach);
		auto instrcution_b = transformQProgToOriginIR(prog_b, mach);
		if (instrcution_a == instrcution_b)
		{
			matched = true;
		}
		else
		{
			matched = false;
		}
	}
	if (criteria_matrix_circuit)
	{
		QuantumMachine* mach = initQuantumMachine(CPU);
		auto prog_a = createEmptyQProg();
		auto prog_b = createEmptyQProg();
		prog_a << a;
		prog_b << b;
		QStat cir_a = getCircuitMatrix(prog_a);
		QStat cir_b = getCircuitMatrix(prog_b);
		if (QStat_to_Eigen(cir_a).isApprox(QStat_to_Eigen(cir_b)))
		{
			matched = true;
		}
		else
		{
			matched = false;
		}
	}
	return matched;
}

void MatrixToPauli::addtoSimplyCircuit(int i, const std::vector<int>& index, int num)
{
	bool is_zero = false;
	if (index.size() > 0)
	{
		double sum = 0;
		for (int k = 0; k < index.size(); k++)
		{
			sum += m_QMcoe[index[k]];
		}
		if (sum + m_QMcoe[i] == 0)
		{
			is_zero = true;
		}
		else
		{
			m_QMcoeMerged.push_back(sum + m_QMcoe[i]);
		}
	}
	else
	{
		m_QMcoeMerged.push_back(m_QMcoe[i]);
	}
	if (!is_zero)
	{
		m_QMcirMerged.push_back(m_QMcir[i]);
	}
}

void QPanda::matrix_decompose_pualis(QuantumMachine* qvm, const EigenMatrixX& mat, PualiOperatorLinearCombination& linearcom)
{
	if (mat.size() == 0 ||
		(mat.rows() != mat.cols()) ||
		(mat.rows() & (mat.rows() - 1)) != 0
		)
	{
		QCERR_AND_THROW_ERRSTR(invalid_argument, "The input matrix is not a 2^n-dimensional square matrix!");
	}
	vector<double> val(mat.data(), mat.data() + mat.size());
	QMatrix<double> ass(mat.rows(), val);
	ass.initialQMatrix();
	MatrixToPauli Vqe_alg(qvm);
	Vqe_alg.matrixDecompositionNew(ass);
	Vqe_alg.combine_same_circuit();
	std::vector<double> coe = Vqe_alg.getQMcoe();
	std::vector<QCircuit> cir = Vqe_alg.getQMcir();
	for (int i = 0; i < coe.size(); i++)
	{
		linearcom.push_back(make_pair(coe[i], cir[i]));
	}
}


#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Vector3d v(1,0,0);
Vector3d w(1e-4,0,1);
cout << "Here's the vector v:" << endl << v << endl;
cout << "Here's the vector w:" << endl << w << endl;
cout << "v.isOrthogonal(w) returns: " << v.isOrthogonal(w) << endl;
cout << "v.isOrthogonal(w,1e-3) returns: " << v.isOrthogonal(w,1e-3) << endl;

  return 0;
}


// g++ -DNDEBUG -O3 -I.. benchEigenSolver.cpp  -o benchEigenSolver && ./benchEigenSolver
// options:
//  -DBENCH_GMM
//  -DBENCH_GSL -lgsl /usr/lib/libcblas.so.3
//  -DEIGEN_DONT_VECTORIZE
//  -msse2
//  -DREPEAT=100
//  -DTRIES=10
//  -DSCALAR=double

#include <iostream>

#include <Eigen/Core>
#include <Eigen/QR>
#include <bench/BenchUtil.h>
using namespace Eigen;

#ifndef REPEAT
#define REPEAT 1000
#endif

#ifndef TRIES
#define TRIES 4
#endif

#ifndef SCALAR
#define SCALAR float
#endif

typedef SCALAR Scalar;

template <typename MatrixType>
__attribute__ ((noinline)) void benchEigenSolver(const MatrixType& m)
{
  int rows = m.rows();
  int cols = m.cols();

  int stdRepeats = std::max(1,int((REPEAT*1000)/(rows*rows*sqrt(rows))));
  int saRepeats = stdRepeats * 4;

  typedef typename MatrixType::Scalar Scalar;
  typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime> SquareMatrixType;

  MatrixType a = MatrixType::Random(rows,cols);
  SquareMatrixType covMat =  a * a.adjoint();

  BenchTimer timerSa, timerStd;

  Scalar acc = 0;
  int r = internal::random<int>(0,covMat.rows()-1);
  int c = internal::random<int>(0,covMat.cols()-1);
  {
    SelfAdjointEigenSolver<SquareMatrixType> ei(covMat);
    for (int t=0; t<TRIES; ++t)
    {
      timerSa.start();
      for (int k=0; k<saRepeats; ++k)
      {
        ei.compute(covMat);
        acc += ei.eigenvectors().coeff(r,c);
      }
      timerSa.stop();
    }
  }

  {
    EigenSolver<SquareMatrixType> ei(covMat);
    for (int t=0; t<TRIES; ++t)
    {
      timerStd.start();
      for (int k=0; k<stdRepeats; ++k)
      {
        ei.compute(covMat);
        acc += ei.eigenvectors().coeff(r,c);
      }
      timerStd.stop();
    }
  }

  if (MatrixType::RowsAtCompileTime==Dynamic)
    std::cout << "dyn   ";
  else
    std::cout << "fixed ";
  std::cout << covMat.rows() << " \t"
            << timerSa.value() * REPEAT / saRepeats << "s \t"
            << timerStd.value() * REPEAT / stdRepeats << "s";

  #ifdef BENCH_GMM
  if (MatrixType::RowsAtCompileTime==Dynamic)
  {
    timerSa.reset();
    timerStd.reset();

    gmm::dense_matrix<Scalar> gmmCovMat(covMat.rows(),covMat.cols());
    gmm::dense_matrix<Scalar> eigvect(covMat.rows(),covMat.cols());
    std::vector<Scalar> eigval(covMat.rows());
    eiToGmm(covMat, gmmCovMat);
    for (int t=0; t<TRIES; ++t)
    {
      timerSa.start();
      for (int k=0; k<saRepeats; ++k)
      {
        gmm::symmetric_qr_algorithm(gmmCovMat, eigval, eigvect);
        acc += eigvect(r,c);
      }
      timerSa.stop();
    }
    // the non-selfadjoint solver does not compute the eigen vectors
//     for (int t=0; t<TRIES; ++t)
//     {
//       timerStd.start();
//       for (int k=0; k<stdRepeats; ++k)
//       {
//         gmm::implicit_qr_algorithm(gmmCovMat, eigval, eigvect);
//         acc += eigvect(r,c);
//       }
//       timerStd.stop();
//     }

    std::cout << " | \t"
              << timerSa.value() * REPEAT / saRepeats << "s"
              << /*timerStd.value() * REPEAT / stdRepeats << "s"*/ "   na   ";
  }
  #endif

  #ifdef BENCH_GSL
  if (MatrixType::RowsAtCompileTime==Dynamic)
  {
    timerSa.reset();
    timerStd.reset();

    gsl_matrix* gslCovMat = gsl_matrix_alloc(covMat.rows(),covMat.cols());
    gsl_matrix* gslCopy = gsl_matrix_alloc(covMat.rows(),covMat.cols());
    gsl_matrix* eigvect = gsl_matrix_alloc(covMat.rows(),covMat.cols());
    gsl_vector* eigval  = gsl_vector_alloc(covMat.rows());
    gsl_eigen_symmv_workspace* eisymm = gsl_eigen_symmv_alloc(covMat.rows());

    gsl_matrix_complex* eigvectz = gsl_matrix_complex_alloc(covMat.rows(),covMat.cols());
    gsl_vector_complex* eigvalz  = gsl_vector_complex_alloc(covMat.rows());
    gsl_eigen_nonsymmv_workspace* einonsymm = gsl_eigen_nonsymmv_alloc(covMat.rows());

    eiToGsl(covMat, &gslCovMat);
    for (int t=0; t<TRIES; ++t)
    {
      timerSa.start();
      for (int k=0; k<saRepeats; ++k)
      {
        gsl_matrix_memcpy(gslCopy,gslCovMat);
        gsl_eigen_symmv(gslCopy, eigval, eigvect, eisymm);
        acc += gsl_matrix_get(eigvect,r,c);
      }
      timerSa.stop();
    }
    for (int t=0; t<TRIES; ++t)
    {
      timerStd.start();
      for (int k=0; k<stdRepeats; ++k)
      {
        gsl_matrix_memcpy(gslCopy,gslCovMat);
        gsl_eigen_nonsymmv(gslCopy, eigvalz, eigvectz, einonsymm);
        acc += GSL_REAL(gsl_matrix_complex_get(eigvectz,r,c));
      }
      timerStd.stop();
    }

    std::cout << " | \t"
              << timerSa.value() * REPEAT / saRepeats << "s \t"
              << timerStd.value() * REPEAT / stdRepeats << "s";

    gsl_matrix_free(gslCovMat);
    gsl_vector_free(gslCopy);
    gsl_matrix_free(eigvect);
    gsl_vector_free(eigval);
    gsl_matrix_complex_free(eigvectz);
    gsl_vector_complex_free(eigvalz);
    gsl_eigen_symmv_free(eisymm);
    gsl_eigen_nonsymmv_free(einonsymm);
  }
  #endif

  std::cout << "\n";

  // make sure the compiler does not optimize too much
  if (acc==123)
    std::cout << acc;
}

int main(int argc, char* argv[])
{
  const int dynsizes[] = {4,6,8,12,16,24,32,64,128,256,512,0};
  std::cout << "size            selfadjoint       generic";
  #ifdef BENCH_GMM
  std::cout << "        GMM++          ";
  #endif
  #ifdef BENCH_GSL
  std::cout << "       GSL (double + ATLAS)  ";
  #endif
  std::cout << "\n";
  for (uint i=0; dynsizes[i]>0; ++i)
    benchEigenSolver(Matrix<Scalar,Dynamic,Dynamic>(dynsizes[i],dynsizes[i]));

  benchEigenSolver(Matrix<Scalar,2,2>());
  benchEigenSolver(Matrix<Scalar,3,3>());
  benchEigenSolver(Matrix<Scalar,4,4>());
  benchEigenSolver(Matrix<Scalar,6,6>());
  benchEigenSolver(Matrix<Scalar,8,8>());
  benchEigenSolver(Matrix<Scalar,12,12>());
  benchEigenSolver(Matrix<Scalar,16,16>());
  return 0;
}


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

#include <iostream>
#include <memory>
#include <ctime>
#include <cmath>

#include <boost/random.hpp>

#include "GaussianProcess.h"
#include "Kernel.h"
#include "MatrixIO.h"

using namespace gpr;

typedef PeriodicKernel<double>		DPKernelType;
typedef std::shared_ptr<DPKernelType> DPKernelTypePointer;
typedef GaussianProcess<double> DPGaussianProcessType;
typedef std::shared_ptr<DPGaussianProcessType> DPGaussianProcessTypePointer;

typedef PeriodicKernel<float>		KernelType;
typedef std::shared_ptr<KernelType> KernelTypePointer;
typedef GaussianProcess<float> GaussianProcessType;
typedef std::shared_ptr<GaussianProcessType> GaussianProcessTypePointer;

typedef DPGaussianProcessType::VectorType DPVectorType;
typedef DPGaussianProcessType::MatrixType DPMatrixType;
typedef GaussianProcessType::VectorType VectorType;
typedef GaussianProcessType::MatrixType MatrixType;

void Test1(){
    /*
     * Test 1: regression of a periodic signal (1D)
     * - generate a periodic signal, and try to predict some more periods
     * - ground truth is: sin(x)*cos(2.2*sin(x))
     */
    std::cout << "Test 1: periodic signal regression ..." << std::flush;

    // ground truth periodic variable
    auto f = [](double x)->double { return std::sin(x)*std::cos(2.2*std::sin(x)); };

    double interval_start = 0;
    double interval_end = 5 * 2*M_PI; // full interval
    double interval_step = 0.1;

    //--------------------------------------------------------------------------------
    // generating ground truth
    unsigned gt_size = (interval_end-interval_start) / interval_step;
    VectorType y(gt_size);
    for(unsigned i=0; i<gt_size; i++){
        y[i] = f(interval_start + i*interval_step);
    }

    //--------------------------------------------------------------------------------
    // perform training
    double noise = 0.01;
    static boost::minstd_rand randgen(static_cast<unsigned>(time(0)));
    static boost::normal_distribution<> dist(0, noise);
    static boost::variate_generator<boost::minstd_rand, boost::normal_distribution<> > r(randgen, dist);

    double interval_training_end = 2 * 2*M_PI; // interval to train
    unsigned number_of_samples = 50;

    KernelTypePointer k(new KernelType(0.59, 0.5, 0.4)); // scale, period, smoothness
    GaussianProcessTypePointer gp(new GaussianProcessType(k));
    gp->SetSigma(noise); // noise

    // add samples
    double training_step_size = (interval_training_end - interval_start) / number_of_samples;
    for(unsigned i=0; i<number_of_samples; i++){
        VectorType x(1);
        x(0) = interval_start + i*training_step_size;

        VectorType y(1);
        y(0) = f(x(0)) + r();

        gp->AddSample(x, y);
    }
    gp->Initialize();

    //--------------------------------------------------------------------------------
    // predict full intervall
    VectorType y_predict(gt_size);
    for(unsigned i=0; i<gt_size; i++){
        VectorType x(1);
        x(0) = interval_start + i*interval_step;
        y_predict[i] = gp->Predict(x)(0);
    }

    double err = (y-y_predict).norm();
    if(err>0.4){
        std::stringstream ss; ss<<err; throw ss.str();
    }
    else{
        std::cout << " [passed]." << std::endl;
    }

}

void Test2(){
    /*
     * Test 2: gaussian process save and load
     */
    std::cout << "Test 2: save/load gaussian process... " << std::flush;
    KernelTypePointer k(new KernelType(3.24, 2.2, 0.3));
    GaussianProcessTypePointer gp(new GaussianProcessType(k));
    gp->SetSigma(0);

    unsigned number_of_samples = 10;

    // add training samples
    for(unsigned i=0; i<number_of_samples; i++){
        VectorType x(2);
        x(0) = x(1) = i * 2*M_PI/number_of_samples;

        VectorType y(2);
        y(0) = std::sin(x(0));
        y(1) = std::cos(x(1));

        gp->AddSample(x,y);
    }
    gp->Initialize();

    gp->Save("/tmp/gp_io_test-");

    KernelTypePointer k_dummy(new KernelType(1, 1, 1));
    GaussianProcessTypePointer gp_read(new GaussianProcessType(k_dummy));
    gp_read->Load("/tmp/gp_io_test-");

    if(*gp.get() == *gp_read.get()){
        std::cout << " [passed]." << std::endl;
    }
    else{
        throw std::string("gps are not equal");
    }
}

void Test3(){
    std::cout << "Test 3: parameter test..." << std::flush;

    KernelTypePointer k(new KernelType(4, 2.5, 0.01)); // scale, period, smoothness
    KernelTypePointer k2(new KernelType(1, 1, 1)); // scale, period, smoothness

    k2->SetParameters(k->GetParameters());

    if((*k) != (*k2)){
        throw std::string("kernels are not equal");
    }
    else{
        std::cout << " [passed]." << std::endl;
    }
}

int main (int argc, char *argv[]){
    std::cout << "Periodic kernel test: " << std::endl;
    try{
        Test1();
        Test2();
        Test3();
    }
    catch(std::string& s){
        std::cout << "[failed] Error: " << s << std::endl;
        return -1;
    }

    return 0;
}


/*
 * L2H1.cpp
 *
 *  Created on: 28.05.2019
 *      Author: thies
 */

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

using namespace dealii;

namespace wavepi {
namespace norms {

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

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

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

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

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

  return std::sqrt(result);
}

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

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

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

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

  return std::sqrt(result);
}

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

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

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

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

  return result;
}

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

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

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

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

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

    u[i] *= factor;
  }
}

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

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

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

    u[i] /= factor;
  }
}

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

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

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

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


#pragma once

#include <paradiseo/eo/eo>

#include <Eigen/Dense>
#include <array>
#include <cassert>

#include "aos/adaptive_operator_selection.hpp"
#include "fla/FitnessLandscapeMetric.hpp"
#include "global.hpp"

class ProblemContext : public eoFunctorBase {
  std::vector<FitnessLandscapeMetric*> metrics;

 public:
  auto compute() -> std::vector<double> {
    std::vector<double> values;
    for (auto& metric : metrics) {
      values.push_back(metric->compute());
    }
    return values;
  }

  void add(FitnessLandscapeMetric& metric) { metrics.emplace_back(&metric); }

  [[nodiscard]] auto size() const -> int { return metrics.size(); }
};

using Eigen::MatrixXd;
using Eigen::VectorXd;

template <class OpT>
class LinUCB : public OperatorSelection<OpT> {
  const double          alpha;
  ProblemContext&       context;
  std::vector<MatrixXd> A;
  std::vector<VectorXd> b;
  std::vector<VectorXd> theta;
  VectorXd              x;
  VectorXd              p;
  int                   opIdx = -1;

  protected:

  auto selectOperatorIdx() -> int override {
    p.maxCoeff(&opIdx);
    return opIdx;
  }

 public:
  LinUCB(std::vector<OpT>             operators,
         ProblemContext&              context,
         const double                 alpha = 0.3)
      : OperatorSelection<OpT>{operators},
        alpha{alpha},
        context{context},
        A{operators.size(), MatrixXd::Identity(context.size(), context.size())},
        b{operators.size(), VectorXd::Zero(context.size())},
        theta{operators.size(), VectorXd{context.size()}},
        x{context.size()},
        p{VectorXd::Constant(operators.size(), 0.5)} {
    assert(alpha >= 0);
  }

  void update() final{};

  void doFeedback(double reward) final {
    if (opIdx == -1)
      return;
    std::vector<double> features = context.compute();
    x = Eigen::Map<VectorXd>(features.data(), features.size());

    A[opIdx]     = A[opIdx] + x * x.transpose();
    b[opIdx]     = b[opIdx] + reward * x;
    theta[opIdx] = A[opIdx].colPivHouseholderQr().solve(b[opIdx]);

    double prod = x.transpose() * A[opIdx] * x;
    p(opIdx)    = theta[opIdx].transpose() * x + alpha * std::sqrt(prod);
  }

  auto printOn(std::ostream& os) -> std::ostream& final {
    os << "  strategy: LinUCB MAB\n"
       << "  alpha: " << alpha << '\n';
    return os;
  }
};


#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>


/*
 *
 * Copyright Toon Knapen, Karl Meerbergen & Kresimir Fresl 2003
 * Copyright Thomas Klimpel 2008
 *
 * 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)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_HBEVX_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_HBEVX_HPP

#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    ///////////////////////////////////////////////////////////////////
    //
    // Eigendecomposition of a banded Hermitian matrix.
    //
    ///////////////////////////////////////////////////////////////////

    /*
     * hbevx() computes the eigenvalues and optionally the associated
     * eigenvectors of a banded Hermitian matrix A. A matrix is Hermitian
     * when herm( A ) == A. When A is real, a Hermitian matrix is also
     * called symmetric.
     *
     * The eigen decomposition is A = U S * herm(U)  where  U  is a
     * unitary matrix and S is a diagonal matrix. The eigenvalues of A
     * are on the main diagonal of S. The eigenvalues are real.
     */

    /*
     * If uplo=='L' only the lower triangular part is stored.
     * If uplo=='U' only the upper triangular part is stored.
     *
     * The matrix is assumed to be stored in LAPACK band format, i.e.
     * matrices are stored columnwise, in a compressed format so that when e.g. uplo=='U'
     * the (i,j) element with j>=i is in position  (i-j) + j * (KD+1) + KD  where KD is the
     * half bandwidth of the matrix. For a triadiagonal matrix, KD=1, for a diagonal matrix
     * KD=0.
     * When uplo=='L', the (i,j) element with j>=i is in position  (i-j) + j * (KD+1).
     *
     * The matrix A is thus a rectangular matrix with KD+1 rows and N columns.
     */

    namespace detail {
      inline
      void hbevx (
        char const jobz, char const range, char const uplo, integer_t const n, integer_t const kd,
        float* ab, integer_t const ldab, float* q, integer_t const ldq,
        float const vl, float const vu, integer_t const il, integer_t const iu,
        float const abstol, integer_t& m,
        float* w, float* z, integer_t const ldz,
        float* work, integer_t* iwork, integer_t* ifail, integer_t& info)
      {
        LAPACK_SSBEVX (
          &jobz, &range, &uplo, &n, &kd, ab, &ldab, q, &ldq,
          &vl, &vu, &il, &iu, &abstol, &m,
          w, z, &ldz,
          work, iwork, ifail, &info);
      }

      inline
      void hbevx (
        char const jobz, char const range, char const uplo, integer_t const n, integer_t const kd,
        double* ab, integer_t const ldab, double* q, integer_t const ldq,
        double const vl, double const vu, integer_t const il, integer_t const iu,
        double const abstol, integer_t& m,
        double* w, double* z, integer_t const ldz,
        double* work, integer_t* iwork, integer_t* ifail, integer_t& info)
      {
        LAPACK_DSBEVX (
          &jobz, &range, &uplo, &n, &kd, ab, &ldab, q, &ldq,
          &vl, &vu, &il, &iu, &abstol, &m,
          w, z, &ldz,
          work, iwork, ifail, &info);
      }

      inline
      void hbevx (
        char const jobz, char const range, char const uplo, integer_t const n, integer_t const kd,
        traits::complex_f* ab, integer_t const ldab, traits::complex_f* q, integer_t const ldq,
        float const vl, float const vu, integer_t const il, integer_t const iu,
        float const abstol, integer_t& m,
        float* w, traits::complex_f* z, integer_t const ldz,
        traits::complex_f* work, float* rwork, integer_t* iwork, integer_t* ifail, integer_t& info)
      {
        LAPACK_CHBEVX (
          &jobz, &range, &uplo, &n, &kd, traits::complex_ptr(ab), &ldab,
          traits::complex_ptr(q), &ldq,
          &vl, &vu, &il, &iu, &abstol, &m,
          w, traits::complex_ptr(z), &ldz,
          traits::complex_ptr(work), rwork, iwork, ifail, &info);
      }

      inline
      void hbevx (
        char const jobz, char const range, char const uplo, integer_t const n, integer_t const kd,
        traits::complex_d* ab, integer_t const ldab, traits::complex_d* q, integer_t const ldq,
        double const vl, double const vu, integer_t const il, integer_t const iu,
        double const abstol, integer_t& m,
        double* w, traits::complex_d* z, integer_t const ldz,
        traits::complex_d* work, double* rwork, integer_t* iwork, integer_t* ifail, integer_t& info)
      {
        LAPACK_ZHBEVX (
          &jobz, &range, &uplo, &n, &kd, traits::complex_ptr(ab), &ldab,
          traits::complex_ptr(q), &ldq,
          &vl, &vu, &il, &iu, &abstol, &m,
          w, traits::complex_ptr(z), &ldz,
          traits::complex_ptr(work), rwork, iwork, ifail, &info);
      }
    }

    namespace detail {
      template <int N>
      struct Hbevx{};

      /// Handling of workspace in the case of one workarray.
      template <>
      struct Hbevx< 1 > {
        template <typename T, typename R>
        void operator() (char const jobz, char const range, char const uplo, integer_t const n,
          integer_t const kd, T* ab, integer_t const ldab, T* q, integer_t const ldq,
          R vl, R vu, integer_t const il, integer_t const iu, R abstol, integer_t& m,
          R* w, T* z, integer_t const ldz,
          minimal_workspace, integer_t* ifail, integer_t& info ) const {

          traits::detail::array<T> work( 7*n );
          traits::detail::array<integer_t> iwork( 5*n );
          hbevx( jobz, range, uplo, n, kd, ab, ldab, q, ldq,
            vl, vu, il, iu, abstol, m,
            w, z, ldz,
            traits::vector_storage( work ),
            traits::vector_storage (iwork),
            ifail, info );
        }

        template <typename T, typename R>
        void operator() (char const jobz, char const range, char const uplo, integer_t const n,
          integer_t const kd, T* ab, integer_t const ldab, T* q, integer_t const ldq,
          R vl, R vu, integer_t const il, integer_t const iu, R abstol, integer_t& m,
          R* w, T* z, integer_t const ldz,
          optimal_workspace, integer_t* ifail, integer_t& info ) const {

          traits::detail::array<T> work( 7*n );
          traits::detail::array<integer_t> iwork( 5*n );
          hbevx( jobz, range, uplo, n, kd, ab, ldab, q, ldq,
            vl, vu, il, iu, abstol, m,
            w, z, ldz,
            traits::vector_storage( work ),
            traits::vector_storage (iwork),
            ifail, info );
        }

        template <typename T, typename R, typename W, typename WI>
        void operator() (char const jobz, char const range, char const uplo, integer_t const n,
          integer_t const kd, T* ab, integer_t const ldab, T* q, integer_t const ldq,
          R vl, R vu, integer_t const il, integer_t const iu, R abstol, integer_t& m,
          R* w, T* z, integer_t const ldz,
          detail::workspace2<W, WI> work,
          integer_t* ifail, integer_t& info ) const {

          assert( traits::vector_size( work.select(T()) )         >= 7*n );
          assert( traits::vector_size( work.select(integer_t()) ) >= 5*n );
          hbevx( jobz, range, uplo, n, kd, ab, ldab, q, ldq,
            vl, vu, il, iu, abstol, m,
            w, z, ldz,
            traits::vector_storage( work.select(T()) ),
            traits::vector_storage( work.select(integer_t()) ),
            ifail, info );
        }
      }; // Hbevx< 1 >

      /// Handling of workspace in the case of two workarrays.
      template <>
      struct Hbevx< 2 > {
        template <typename T, typename R>
        void operator() (char const jobz, char const range, char const uplo, integer_t const n,
          integer_t const kd, T* ab, integer_t const ldab, T* q, integer_t const ldq,
          R vl, R vu, integer_t const il, integer_t const iu, R abstol, integer_t& m,
          R* w, T* z, integer_t const ldz,
          minimal_workspace, integer_t* ifail, integer_t& info ) const {

          traits::detail::array<T> work( n );
          traits::detail::array<R> rwork( 7*n );
          traits::detail::array<integer_t> iwork( 5*n );
          hbevx( jobz, range, uplo, n, kd, ab, ldab, q, ldq,
            vl, vu, il, iu, abstol, m,
            w, z, ldz,
            traits::vector_storage( work ),
            traits::vector_storage( rwork ),
            traits::vector_storage (iwork),
            ifail, info );
        }

        template <typename T, typename R>
        void operator() (char const jobz, char const range, char const uplo, integer_t const n,
          integer_t const kd, T* ab, integer_t const ldab, T* q, integer_t const ldq,
          R vl, R vu, integer_t const il, integer_t const iu, R abstol, integer_t& m,
          R* w, T* z, integer_t const ldz,
          optimal_workspace, integer_t* ifail, integer_t& info ) const {

          traits::detail::array<T> work( n );
          traits::detail::array<R> rwork( 7*n );
          traits::detail::array<integer_t> iwork( 5*n );
          hbevx( jobz, range, uplo, n, kd, ab, ldab, q, ldq,
            vl, vu, il, iu, abstol, m,
            w, z, ldz,
            traits::vector_storage( work ),
            traits::vector_storage( rwork ),
            traits::vector_storage (iwork),
            ifail, info );
        }

        template <typename T, typename R, typename W, typename RW, typename WI>
        void operator() (char const jobz, char const range, char const uplo, integer_t const n,
          integer_t const kd, T* ab, integer_t const ldab, T* q, integer_t const ldq,
          R vl, R vu, integer_t const il, integer_t const iu, R abstol, integer_t& m,
          R* w, T* z, integer_t const ldz,
          detail::workspace3<W, RW, WI> work,
          integer_t* ifail, integer_t& info ) const {

          assert( traits::vector_size( work.select(T()) ) >= n );
          assert( traits::vector_size( work.select(R()) ) >= 7*n );
          assert( traits::vector_size( work.select(integer_t()) ) >= 5*n );
          hbevx( jobz, range, uplo, n, kd, ab, ldab, q, ldq,
            vl, vu, il, iu, abstol, m,
            w, z, ldz,
            traits::vector_storage( work.select(T()) ),
            traits::vector_storage( work.select(R()) ),
            traits::vector_storage( work.select(integer_t()) ),
            ifail, info );
        }
      }; // Hbevx< 2 >
    } // namespace detail

    template <typename AB, typename Q, typename R, typename Z, typename W, typename IFail, typename Work>
    int hbevx( char const jobz, char const range, AB& ab, Q& q, R vl, R vu, integer_t il, integer_t iu, R abstol, integer_t& m,
      W& w, Z& z, IFail& ifail, Work work ) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<AB>::matrix_structure,
        traits::hermitian_t
      >::value));
#endif

      typedef typename AB::value_type                            value_type ;

      integer_t const n = traits::matrix_size2 (ab);
      assert (n == traits::matrix_size1 (z));
      assert (n == traits::vector_size (w));
      assert (n == traits::vector_size (ifail));
      assert ( jobz=='N' || jobz=='V' );

      integer_t info ;
      detail::Hbevx< n_workspace_args<value_type>::value >() (jobz, range,
        traits::matrix_uplo_tag( ab ), n,
        traits::matrix_upper_bandwidth(ab),
        traits::matrix_storage (ab),
        traits::leading_dimension (ab),
        traits::matrix_storage (q),
        traits::leading_dimension (q),
        vl, vu, il, iu, abstol, m,
        traits::vector_storage (w),
        traits::matrix_storage (z),
        traits::leading_dimension (z),
        work,
        traits::vector_storage (ifail),
        info);
      return info ;
    } // hbevx()
  }

}}}

#endif


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Framework/TestingFramework.hpp"

#include <array>
#include <boost/optional.hpp>
#include <cmath>
#include <cstddef>
#include <memory>
#include <string>
#include <unordered_map>

#include "DataStructures/Tensor/Tensor.hpp"
#include "Domain/CoordinateMaps/TimeDependent/Rotation.hpp"
#include "Domain/FunctionsOfTime/PiecewisePolynomial.hpp"
#include "Framework/TestHelpers.hpp"
#include "Helpers/Domain/CoordinateMaps/TestMapHelpers.hpp"
#include "Utilities/ConstantExpressions.hpp"

class DataVector;
namespace domain::FunctionsOfTime {
class FunctionOfTime;
}  // namespace domain::FunctionsOfTime

namespace {
std::array<double, 2> expected_mapped_point(
    const std::array<double, 2> initial_unmapped_point,
    const double time) noexcept {
  const double rotation_angle = square(time);
  return {{initial_unmapped_point[0] * cos(rotation_angle) -
               initial_unmapped_point[1] * sin(rotation_angle),
           initial_unmapped_point[0] * sin(rotation_angle) +
               initial_unmapped_point[1] * cos(rotation_angle)}};
}

std::array<double, 2> expected_frame_velocity(
    const std::array<double, 2> initial_unmapped_point,
    const double time) noexcept {
  const double rotation_angle = square(time);
  const double angular_velocity = 2.0 * time;
  return {
      {initial_unmapped_point[0] * -sin(rotation_angle) * angular_velocity -
           initial_unmapped_point[1] * cos(rotation_angle) * angular_velocity,
       initial_unmapped_point[0] * cos(rotation_angle) * angular_velocity +
           initial_unmapped_point[1] * -sin(rotation_angle) *
               angular_velocity}};
}
}  // namespace

namespace domain {
SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.RotationTimeDep",
                  "[Domain][Unit]") {
  double t{-1.0};
  const double dt{0.6};
  const double final_time{4.0};
  constexpr size_t deriv_order{3};
  constexpr size_t spatial_dim{2};

  using Polynomial = domain::FunctionsOfTime::PiecewisePolynomial<deriv_order>;
  using FoftPtr = std::unique_ptr<domain::FunctionsOfTime::FunctionOfTime>;

  const std::string f_of_t_name{"rotation_angle"};
  const std::array<DataVector, deriv_order + 1> init_func{
      {{1.0}, {-2.0}, {2.0}, {0.0}}};
  std::unordered_map<std::string, FoftPtr> f_of_t_list{};
  f_of_t_list[f_of_t_name] =
      std::make_unique<Polynomial>(t, init_func, final_time + dt);

  const CoordinateMaps::TimeDependent::Rotation<spatial_dim> rotation_map{
      f_of_t_name};
  const auto rotation_map_deserialized =
      serialize_and_deserialize(rotation_map);

  const std::array<double, 2> initial_unmapped_point{{3.2, 4.5}};

  while (t < final_time) {
    const std::array<double, 2> expected{
        expected_mapped_point(initial_unmapped_point, t)};
    CHECK_ITERABLE_APPROX(rotation_map(initial_unmapped_point, t, f_of_t_list),
                          expected);
    CHECK_ITERABLE_APPROX(rotation_map.inverse(expected, t, f_of_t_list).get(),
                          initial_unmapped_point);
    CHECK_ITERABLE_APPROX(
        rotation_map.frame_velocity(initial_unmapped_point, t, f_of_t_list),
        expected_frame_velocity(initial_unmapped_point, t));

    CHECK_ITERABLE_APPROX(
        rotation_map_deserialized(initial_unmapped_point, t, f_of_t_list),
        expected);
    CHECK_ITERABLE_APPROX(
        rotation_map_deserialized.inverse(expected, t, f_of_t_list).get(),
        initial_unmapped_point);
    CHECK_ITERABLE_APPROX(rotation_map_deserialized.frame_velocity(
                              initial_unmapped_point, t, f_of_t_list),
                          expected_frame_velocity(initial_unmapped_point, t));

    const auto jac{
        rotation_map.jacobian(initial_unmapped_point, t, f_of_t_list)};
    const auto inv_jac{
        rotation_map.inv_jacobian(initial_unmapped_point, t, f_of_t_list)};
    const double cos_t_squared{cos(square(t))};
    const double sin_t_squared{sin(square(t))};

    CHECK(get<0, 0>(jac) == approx(cos_t_squared));
    CHECK(get<0, 1>(jac) == approx(-sin_t_squared));
    CHECK(get<1, 0>(jac) == approx(sin_t_squared));
    CHECK(get<1, 1>(jac) == approx(cos_t_squared));

    CHECK(get<0, 0>(inv_jac) == approx(cos_t_squared));
    CHECK(get<0, 1>(inv_jac) == approx(sin_t_squared));
    CHECK(get<1, 0>(inv_jac) == approx(-sin_t_squared));
    CHECK(get<1, 1>(inv_jac) == approx(cos_t_squared));

    const auto jac_deserialized{rotation_map_deserialized.jacobian(
        initial_unmapped_point, t, f_of_t_list)};
    const auto inv_jac_deserialized{rotation_map_deserialized.inv_jacobian(
        initial_unmapped_point, t, f_of_t_list)};
    CHECK(get<0, 0>(jac_deserialized) == approx(cos_t_squared));
    CHECK(get<0, 1>(jac_deserialized) == approx(-sin_t_squared));
    CHECK(get<1, 0>(jac_deserialized) == approx(sin_t_squared));
    CHECK(get<1, 1>(jac_deserialized) == approx(cos_t_squared));

    CHECK(get<0, 0>(inv_jac_deserialized) == approx(cos_t_squared));
    CHECK(get<0, 1>(inv_jac_deserialized) == approx(sin_t_squared));
    CHECK(get<1, 0>(inv_jac_deserialized) == approx(-sin_t_squared));
    CHECK(get<1, 1>(inv_jac_deserialized) == approx(cos_t_squared));

    t += dt;
  }

  // Check inequivalence operator
  CHECK_FALSE(rotation_map != rotation_map);
  CHECK_FALSE(rotation_map_deserialized != rotation_map_deserialized);

  // Check serialization
  CHECK(rotation_map == rotation_map_deserialized);
  CHECK_FALSE(rotation_map != rotation_map_deserialized);

  test_coordinate_map_argument_types(rotation_map, initial_unmapped_point, t,
                                     f_of_t_list);
  CHECK(
      not CoordinateMaps::TimeDependent::Rotation<spatial_dim>{}.is_identity());
}
}  // namespace domain


#include <iostream>
#include <chrono>
#include <sstream>
#include <fstream>
#include <Eigen/Dense>
#include "Timer.h"
#include "Projection.h"
#include "Volumetric_helper.h"
#include "KinectFrame.h"
#include "KinectSpecs.h"

// Usage: ./NormalEstimate.exe ../../data/room.knt ../../data/room_normals.obj
int main(int argc, char **argv)
{

	if (argc < 3)
	{
		std::cerr
			<< "Missing parameters. Abort."
			<< std::endl
			<< "Usage: ./NormalEstimate.exe ../../data/room.knt ../../data/room_normals.obj"
			<< std::endl;
		return EXIT_FAILURE;
	}

	Timer timer;
	const std::string input_filename = argv[1];
	const std::string output_filename = argv[2];

	try
	{
		timer.start();
		KinectFrame frame;
		KinectFrame::load(input_filename, frame);
		timer.print_interval("Importing kinect frame    : ");

		float fovy = KINECT_V2_FOVY;
		float aspect_ratio = KINECT_V2_DEPTH_ASPECT_RATIO;
		float near_plane = KINECT_V2_DEPTH_MIN;
		float far_plane = KINECT_V2_DEPTH_MAX;

		if (frame.depth_width() != 512)	// it is not kinect version 2. It is version 1
		{
			fovy = KINECT_V1_FOVY;
			aspect_ratio = KINECT_V1_ASPECT_RATIO;
			near_plane = KINECT_V1_DEPTH_MIN;
			far_plane = KINECT_V1_DEPTH_MAX;
		}

		std::vector<Eigen::Vector3f> vertices(frame.depth.size(), Eigen::Vector3f(0, 0, 0));
		std::vector<Eigen::Vector3f> normals(frame.depth.size(), Eigen::Vector3f(0, 0, 1));
		std::vector<Eigen::Vector3i> colors(frame.depth.size(), Eigen::Vector3i(0, 0, 255));

		timer.start();

		Eigen::Vector3f vert_uv, vert_u1v, vert_uv1;

		for (int x = 0; x < frame.depth_width() - 1; ++x)
		{
			for (int y = 0; y < frame.depth_height() - 1; ++y)
			{
				const float depth = frame.depth[y * frame.depth_width() + x];
				const float depth_u1v = frame.depth[y * frame.depth_width() + x + 1];
				const float depth_uv1 = frame.depth[(y + 1) * frame.depth_width() + x];

				if (depth > 0.01 && depth_u1v > 0.01 && depth_uv1 > 0.01)
				{
					vert_uv = window_coord_to_3d(Eigen::Vector2f(x, y), depth, fovy, aspect_ratio, near_plane, far_plane, frame.depth_width(), frame.depth_height());
					vert_u1v = window_coord_to_3d(Eigen::Vector2f(x + 1, y), depth_u1v, fovy, aspect_ratio, near_plane, far_plane, frame.depth_width(), frame.depth_height());
					vert_uv1 = window_coord_to_3d(Eigen::Vector2f(x, y + 1), depth_uv1, fovy, aspect_ratio, near_plane, far_plane, frame.depth_width(), frame.depth_height());

					const Eigen::Vector3f n1 = vert_u1v - vert_uv;
					const Eigen::Vector3f n2 = vert_uv1 - vert_uv;
					const Eigen::Vector3f n = n1.cross(n2).normalized();

					int i = y * frame.depth_width() + x;

					vertices[i] = vert_uv;
					normals[i] = n;
					colors[i] = ((n * 0.5f + Eigen::Vector3f(0.5, 0.5, 0.5)) * 255.0f).cast<int>();
				}
			}
		}
		timer.print_interval("Depth map back projection : ");

		timer.start();
		//export_obj_with_normals(output_filename, vertices, normals);
		export_obj_with_colors(output_filename, vertices, colors);
		timer.print_interval("Exporting output .obj     : ");
	}
	catch (const std::exception& ex)
	{
		std::cerr << "Error: " << ex.what() << std::endl;
	}

	return 0;
}


// STL includes
#include <iostream>
#include <vector>

// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
  boost::no_property, boost::property<boost::edge_weight_t, int> >      weighted_graph;
typedef boost::property_map<weighted_graph, boost::edge_weight_t>::type weight_map;
typedef boost::graph_traits<weighted_graph>::edge_descriptor            edge_desc;
typedef boost::graph_traits<weighted_graph>::vertex_descriptor          vertex_desc;

int dijkstra_dist(const weighted_graph &G, int s, int t) {
  int n = boost::num_vertices(G);
  std::vector<int> dist_map(n);

  boost::dijkstra_shortest_paths(G, s,
    boost::distance_map(boost::make_iterator_property_map(
      dist_map.begin(), boost::get(boost::vertex_index, G))));

  return dist_map[t];
}

int index(int v, int k, int n) {
  return k * n + v;
}

void solve()
{
  int n; std::cin >> n;
  int m; std::cin >> m;
  int k; std::cin >> k;
  int x; std::cin >> x;
  int y; std::cin >> y;

  weighted_graph G;

  int a, b, c, d;
  for (int i = 0; i < m; ++i) {
    std::cin >> a;
    std::cin >> b;
    std::cin >> c;
    std::cin >> d;

    for (int j = 1; j <= k; ++j) {
      boost::add_edge(a + j * n, b + (j - d) * n, c, G);
      boost::add_edge(b + j * n, a + (j - d) * n, c, G);
    }
    boost::add_edge(a, b, c, G);
    boost::add_edge(b, a, c, G);
  }

  std::cout << dijkstra_dist(G, x + k * n, y) << std::endl;
}

int main() {
  std::ios_base::sync_with_stdio(false);
  int t; std::cin >> t;
  for (int i = 0; i < t; ++i) {
    solve();
  }
}


#ifndef MISC_HPP
#define MISC_HPP

#include <Eigen/Core>
// #include <boost/utility.hpp>
// #include <boost/type_traits.hpp>

double fastSigmoid(double x);
double penaltyBoundToInterval(const double& v, const double m, const double& e); //v: The scalar that should be bounded, a: lower bound, b: upper bound,  e: safty margin
double penaltyBoundAngle(double a1, double a2, double a3);

inline double fastSigmoid(double x)
{
  return x / (1 + fabs(x));
}

inline double penaltyBound(const double& v, const double m, const double& e)
{
  if ( (v > m+e)  || (v <= m-e) )
  {
    return (1.0*(v + (m + e)));
  }
  else
  {
    return 0.0;
  }
}

inline double penaltyBoundTime(const double& x, const double m, const double& e)
{
  if ( (x < m)  || (x >= m*e) )
  {
    return 1.0*(x  + m);
  }
  else
  {
    return 0.0;
  }
}

inline double penaltyBoundAngle(double a1, double a2, double a3)
{
  double ret1 , ret2;
  if ( a1 - a2 > M_PI /6 )
    ret1 = (a1 - a2);
  else
    ret1 = 0.;

  if ( a2 - a3 > M_PI /6 )
    ret2 = (a2 - a3);
  else
    ret2 = 0.;

  return (ret1+ret2);
}

#endif


#include "geometry.hpp"
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <boost/functional/hash.hpp>
#include <cmath>
#include <algorithm>

namespace r = ranges;
namespace rv = ranges::views;

ch::matrix ch::rotation_matrix(double theta)
{
	return rotation_matrix(std::cos(theta), std::sin(theta));
}

ch::matrix ch::rotation_matrix(double cos_theta, double sin_theta) {
	ch::matrix rotation;
	rotation <<
		cos_theta, -sin_theta, 0,
		sin_theta, cos_theta, 0,
		0, 0, 1;
	return rotation;
}

ch::matrix ch::translation_matrix(double x, double y) {
	ch::matrix translation;
	translation <<
		1, 0, x,
		0, 1, y,
		0, 0, 1;
	return translation;
}

ch::matrix ch::translation_matrix(const cv::Point2d& pt)
{
	return translation_matrix(pt.x, pt.y);
}

ch::matrix ch::scale_matrix(double x_scale, double y_scale) {
	ch::matrix scale;
	scale <<
		x_scale, 0, 0,
		0, y_scale, 0,
		0, 0, 1;
	return scale;
}

ranges::any_view<ch::polyline> ch::transform(ranges::any_view<polyline> polys, const matrix& mat)
{
	return polys | rv::transform([=](const auto& poly) { return ch::transform(poly, mat); });
}

std::vector<ch::polyline> ch::transform(const std::vector<polyline>& polys, const matrix& mat)
{
	std::vector<ch::polyline> output(polys.size());
	std::transform(polys.begin(), polys.end(), output.begin(),
		[&mat](const auto& poly) {
			return ch::transform(poly, mat);
		}
	);
	return output;
}

ch::point ch::mean_point(const polyline& poly)
{
	auto x = 0.0;
	auto y = 0.0;
	for (const auto& pt : poly) {
		x += pt.x;
		y += pt.y;
	}
	return {
		x / poly.size(),
		y / poly.size()
	};
}

ch::polyline ch::transform(const polyline& poly, const matrix& mat)
{
	ch::polyline output(poly.size());
	std::transform(poly.begin(), poly.end(), output.begin(),
		[&mat](const auto& p) {return transform(p, mat); }
	);
	return output;
}

ch::point ch::transform(const point& pt, const matrix& mat)
{
	vec v;
	v << pt.x, pt.y, 1.0;
	v = mat * v;
	return { v[0], v[1] };
}

void ch::paint_polyline(cv::Mat& mat, const polyline& poly, double thickness, int color, point offset)
{
	std::vector<cv::Point> int_pts(poly.size());
	std::transform(poly.begin(), poly.end(), int_pts.begin(),
		[offset](const auto& p) {
			return cv::Point(
				static_cast<int>(std::round(p.x + offset.x)),
				static_cast<int>(std::round(p.y + offset.y))
			);
		}
	);
	auto npts = int_pts.size();
	cv::polylines(mat, int_pts, false, color, thickness, 8, 0);
}

double ch::euclidean_distance(const point& pt1, const point& pt2)
{
	auto x_diff = pt2.x - pt1.x;
	auto y_diff = pt2.y - pt1.y;
	return std::sqrt(x_diff * x_diff + y_diff * y_diff);
}

std::size_t ch::point_hasher::operator()(const cv::Point& p) const
{
	std::size_t seed = 0;
	boost::hash_combine(seed, p.x);
	boost::hash_combine(seed, p.y);

	return seed;
}


//////////////////////////////////////////////////////////////////////////////
// random::poisson_ext::detail::math.hpp                                    //
//                                                                          //
//                                                                          //
//  (C) Copyright 2010 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_RANDOM_POISSON_EXT_DETAIL_MATH_HPP_ER_2010
#define BOOST_RANDOM_POISSON_EXT_DETAIL_MATH_HPP_ER_2010
#include <cmath>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/factorials.hpp>

#include <boost/numeric/conversion/converter.hpp>
#include <boost/numeric/conversion/bounds.hpp>
#include <boost/limits.hpp>

namespace boost{
namespace random{
namespace poisson{
namespace devroye{
namespace detail{

	// Math functions or constants are gathered here, so that any implementation
    // change to one of them need not be replicated throughout the code.
 	template<typename Int,typename T,typename P>
	struct math{
    	typedef Int int_type;
    	typedef T float_type;

		static const float_type pi(){
        	static const float_type val
            	= boost::math::constants::pi<float_type>();
            return val;
        }
        static const float_type eps(){
        	return boost::numeric::bounds<float_type>::smallest();
        }
        static bool is_strictly_negative(const float_type& x){
        	return ( !( x >= (-eps()) ) );
        }
        template<typename E>
        static float_type pow(const float_type& x,const E& k){
        	return std::pow(x,k);
        }
        static float_type exp(const float_type& x){
        	return std::exp(x);
        }
        static float_type sqrt(const float_type& x){
        	return std::sqrt(x);
        }
		static float_type floor(const float_type& x){
        	float_type val = std::floor(x);
        	return val;
        }
		static float_type ceil(const float_type& x){
        	float_type val = std::ceil(x);
        	return val;
        }
        static float_type log1p(const float_type& x){
        	return boost::math::log1p(x);
        }
        static float_type log1p(const float_type& x,const P& p){
        	return boost::math::log1p(x,p);
        }
        static float_type log(const float_type& x){
        	return std::log(x);
        }
        static float_type factorial(const int_type& i,const P& p){
        	return boost::math::factorial<float_type>(i,p);
        }

		template<typename To,typename From>
        static To convert(const From& t){
            typedef boost::numeric::converter<To,From> to_;
            return to_::convert(t);

        }
		static float_type to_float(const int_type& i){
            return convert<float_type>(i);
        }
		static int_type to_int(const float_type& x){
            return convert<int_type>(x);
        }
	};

}// math
}// devroye
}// poisson
}// random
}// boost

#endif


// STL includes
#include <iostream>
#include <vector>

// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/biconnected_components.hpp>

namespace boost {
    struct edge_component_t {
        enum { num = 555 };
        typedef edge_property_tag kind;
    } edge_component;
}

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, boost::no_property, boost::property<boost::edge_component_t, std::size_t >> graph;
typedef boost::graph_traits<graph>::edge_descriptor edge_desc;
typedef boost::graph_traits<graph>::vertex_descriptor vertex_desc;

using namespace std;

struct edge {
    int u;
    int v;

    bool operator<(edge &o) const {
        return u < o.u || (u == o.u && v < o.v);
    }
};

void solve() {
    int n;
    cin >> n;
    int m;
    cin >> m;

    graph G(n);

    int u, v;
    for (int i = 0; i < m; ++i) {
        cin >> u;
        cin >> v;
        boost::add_edge(u, v, G);
    }

    boost::property_map<graph, boost::edge_component_t>::type component = get(boost::edge_component, G);
    int ncc = biconnected_components(G, component);

    vector<vector<edge_desc>> edgesOfComponent(ncc, vector<edge_desc>());

    boost::graph_traits<graph>::edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = edges(G); ei != ei_end; ++ei) {
        edgesOfComponent[component[*ei]].push_back(*ei);
    }

    vector<edge> result;
    for (int i = 0; i < ncc; ++i) {
        if (edgesOfComponent[i].size() == 1) {
            u = source(edgesOfComponent[i][0], G);
            v = target(edgesOfComponent[i][0], G);
            if (v < u) {
                u = target(edgesOfComponent[i][0], G);
                v = source(edgesOfComponent[i][0], G);
            }
            result.push_back({u, v});
        }
    }
    sort(result.begin(), result.end());

    cout << result.size() << endl;
    for (auto e : result) {
        cout << e.u << " " << e.v << endl;
    }
}

int main() {
    int t;
    cin >> t;
    for (int i = 0; i < t; ++i) {
        solve();
    }
}