#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 ((((abs(matrix(rdx, cdx).real()) < MAX_MATRIX_PRECISION) && (abs(matrix(rdx, cdx).imag()) < MAX_MATRIX_PRECISION)) && (idx != opts - 1)) ||
				(((abs(matrix(cdx + 1, cdx).real() - 1.0) < MAX_MATRIX_PRECISION) && (abs(matrix(cdx + 1, cdx).imag()) < MAX_MATRIX_PRECISION)) && (idx == opts - 1)))
			/*if ((EigenComplexT(0, 0) == matrix(rdx, cdx) && (idx != opts - 1)) ||
				(EigenComplexT(1, 0) == matrix(cdx + 1, 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(!V2.isApprox(EigenMatrixXc::Identity(2, 2), MAX_MATRIX_PRECISION))
	//if (EigenMatrixXc::Identity(2, 2) != V2)
	{
		QStat M2 = { (qcomplex_t)((EigenComplexT)1.0 / V2(0,0)), (qcomplex_t)(V2(0,1)),
					 (qcomplex_t)(V2(1,0)) , (qcomplex_t)((EigenComplexT)1.0 / V2(1,1))};

		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)
{
	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)
		{
			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))
			{
				continue;
			}

			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;
			}

			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;
};

/*******************************************************************
*                      class HQRDecompose
* Householder QR-decompose
* refer to <Quantum circuits synthesis using Householder transformations>(https://arxiv.org/abs/2004.07710v1)
********************************************************************/
class HQRDecompose
{
public:
	HQRDecompose()
		:m_dimension(0)
	{}
	~HQRDecompose() {}

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

		const auto mat_dimension = src_mat.rows();
		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.");
		}

		// do Householder QR_decompose
		m_dimension = mat_dimension;
		m_qubits = qubits;
		auto start = system_clock::now();
		Householder_QR_decompose(src_mat);

#if PRINT_TRACE
		auto end = system_clock::now();
		auto duration = duration_cast<microseconds>(end - start);
		PTrace("Total used: %f s",
			double(duration.count()) * microseconds::period::num / microseconds::period::den);
#endif

		return m_result_cir;
	}

protected:
	void print_vec(const QStat& vec)
	{
		for (auto i = 0; i < vec.size(); ++i)
		{
			printf("(%-g, %-g), ", vec[i].real(), vec[i].imag());
		}

		printf("\n");
	}

	void Householder_QR_decompose(EigenMatrixXc src_mat)
	{
		const auto& lines = m_dimension;
		QStat tmp_mat_R(m_dimension * m_dimension);
		QCircuit cir_Q;
		for (size_t cur_col = 0; cur_col < lines; ++cur_col)
		{
			//printf("On column %ld\n", cur_col);
			const auto cur_col_size = lines - cur_col;
			QStat cur_col_vec(cur_col_size);
			QStat cur_col_vec_dagger(cur_col_size);
			qstate_type norm = 0;
			for (size_t cur_row = cur_col; cur_row < lines; ++cur_row)
			{
				cur_col_vec[cur_row - cur_col] = -src_mat(cur_row, cur_col);
				norm += (cur_col_vec[cur_row - cur_col].real() * cur_col_vec[cur_row - cur_col].real() +
					cur_col_vec[cur_row - cur_col].imag() * cur_col_vec[cur_row - cur_col].imag());
			}

			norm = sqrt(norm);
			const double angle = arg(cur_col_vec[0]);
			cur_col_vec[0] -= exp(qcomplex_t(0, angle)); // ?
			//vec[0] = exp(complex_t(0, 1.0 * angle)) * (sqrt(vec[0].real() * vec[0].real() + vec[0].imag() * vec[0].imag()) - tmp_x);

			norm = 0.0;
			for (size_t i = 0; i < cur_col_size; ++i)
			{
				norm += (cur_col_vec[i].real() * cur_col_vec[i].real() + cur_col_vec[i].imag() * cur_col_vec[i].imag());
			}

			if (norm > 1e-7)
			{
				// vec Dagger
				for (size_t i = 0; i < cur_col_size; ++i)
				{
					qstate_type real_v = cur_col_vec[i].real();
					qstate_type imag_v = cur_col_vec[i].imag();
					//cur_col_vec[i] = qcomplex_t(real_v, imag_v);
					cur_col_vec_dagger[i] = qcomplex_t(real_v, -imag_v);
				}
#if PRINT_TRACE
				printf("The vec:\n");
				print_vec(cur_col_vec);
				printf("The vec_dagger:\n");
				print_vec(cur_col_vec_dagger);
				printf(":::::::::::::::::::::::::::::\n");
#endif
				//sestavit matici P
				EigenMatrixXc matrix_p(cur_col_size, cur_col_size);
				for (size_t k = 0; k < cur_col_size; ++k)
				{
					for (size_t h = 0; h < cur_col_size; ++h)
					{
						if (k == h)
						{
							matrix_p(k, k) = qcomplex_t(1, 0) - qcomplex_t(2, 0) * cur_col_vec[k] * cur_col_vec_dagger[h] / norm;

						}
						else
						{
							matrix_p(k, h) = -qcomplex_t(2, 0) * cur_col_vec[k] * cur_col_vec_dagger[h] / norm;
						}
					}
				}
				PTrace("-----tmp matrixP:\n");
				PTraceMat(matrix_p);
				PTrace("tmp matrixP end -----------:\n");

				auto norm_sqr = sqrt(norm);
				for (size_t i = 0; i < cur_col_size; ++i)
				{
					cur_col_vec[i] /= norm_sqr;
				}
				cir_Q << build_cir_Pi(cur_col_vec);
				/*using testMat = Eigen::Matrix<qcomplex_t, -1, -1, Eigen::RowMajor>;
				testMat mat_test_1 = testMat::Map(&matrix_p[0], cur_col_size, cur_col_size);*/
				/*testMat mat_test_2 = testMat::Map(&src_mat[0], m_dimension, m_dimension);*/
				EigenMatrixXc mat_test_3 = src_mat.bottomRightCorner(m_dimension - cur_col, m_dimension - cur_col);
				matrix_p *= mat_test_3;
				//src_mat.block(cur_col, cur_col, m_dimension - cur_col, m_dimension - cur_col) *= matrix_p;
				//src_mat.bottomRightCorner(m_dimension - cur_col, m_dimension - cur_col) = matrix_p;
				src_mat.block(cur_col, cur_col, m_dimension - cur_col, m_dimension - cur_col) = matrix_p;
				PTrace("-----tmp matrixA:\n");
				PTraceMat(src_mat);
				PTrace("tmp matrixA end -----------:\n");
			}
		}

		//QCircuit last_cir_D = matrix_decompose(m_qubits, src_mat);
		QStat mat_r(src_mat.data(), src_mat.data() + src_mat.size());
		QCircuit last_cir_D = diagonal_matrix_decompose(m_qubits, mat_r);
		m_result_cir << last_cir_D << cir_Q.dagger();
	}

	QCircuit build_cir_Pi(const QStat& cur_col_vec)
	{
		QStat full_cur_vec(m_dimension - cur_col_vec.size(), qcomplex_t(0, 0));
		full_cur_vec.insert(full_cur_vec.end(), cur_col_vec.begin(), cur_col_vec.end());
		if (full_cur_vec.size() != m_dimension)
		{
			QCERR_AND_THROW_ERRSTR(run_fail, "Error: current vector size error on HQRDecompose.");
		}

		QCircuit cir_swap_qubits;
		for (size_t i = 0; (i * 2) < (m_qubits.size() - 1); ++i)
		{
			cir_swap_qubits << SWAP(m_qubits[i], m_qubits[m_qubits.size() - 1 - i]);
		}

		std::vector<double> ui_mod(m_dimension);
		std::vector<double> ui_angle(m_dimension);
		double tatal = 0.0;
		for (size_t i = 0; i < m_dimension; ++i)
		{
			auto tmp_m = full_cur_vec[i].real() * full_cur_vec[i].real() + full_cur_vec[i].imag() * full_cur_vec[i].imag();
			tatal += tmp_m;
			ui_mod[i] = sqrt(tmp_m);
			ui_angle[i] = arg(full_cur_vec[i]);
		}

		QStat mat_d(m_dimension * m_dimension, qcomplex_t(0, 0));
		for (size_t i = 0; i < m_dimension; ++i)
		{
			mat_d[i + i * m_dimension] = exp(qcomplex_t(0, ui_angle[i]));
		}

		QCircuit cir_d = diagonal_matrix_decompose(m_qubits, mat_d);

		PTrace("cir_d:\n");
		PTraceCircuit(cir_d);

#if PRINT_TRACE
		const auto mat_test_d = getCircuitMatrix(cir_d);
		PTrace("mat_test_d:\n");
		PTraceCircuit(mat_test_d);
		if (mat_test_d == mat_d)
		{
			cout << "matrix decompose okkkkkkkkkkkkkkk" << endl;
		}
		else
		{
			cout << "ffffffffffffffffffffailed on matrix decompose." << endl;
		}
#endif

		QCircuit cir_y = build_cir_b(m_qubits, ui_mod);
		QCircuit cir_P;
		cir_P << cir_y << cir_swap_qubits  << cir_d << cir_swap_qubits;

		QCircuit cir_DG = zero_phase_shift_cir();
		QCircuit cir_pi;
		cir_pi << cir_swap_qubits  << cir_P.dagger() << cir_DG << cir_P << cir_swap_qubits;

		return cir_pi;
	}

	QCircuit zero_phase_shift_cir()
	{
		QCircuit cir_DG;
		QVec tmp_qubits = m_qubits;
		tmp_qubits.pop_back();
		cir_DG << applyQGate(m_qubits, X) << Z(m_qubits.back()).control(tmp_qubits) << applyQGate(m_qubits, X);

		return cir_DG;
	}

	QCircuit build_cir_b(QVec qubits, const std::vector<double>& b)
	{
		return amplitude_encode(qubits, b);
	}

private:
	QVec m_qubits;
	size_t m_dimension;
	QCircuit m_result_cir;
};

static QCircuit Householder_qr_matrix_decompose(QVec qubits, const EigenMatrixXc& src_mat)
{
	return HQRDecompose().decompose(qubits, src_mat);
}

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

	return matrix_decompose(qubits, tmp_mat, de_mode);
}

QCircuit QPanda::matrix_decompose(QVec qubits, EigenMatrixXc& src_mat, DecompositionMode de_mode/* = HOUSEHOLDER_QR*/)
{
	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.");
	}

	QCircuit output_circuit;
	switch (de_mode)
	{
	case HOUSEHOLDER_QR:
		output_circuit = Householder_qr_matrix_decompose(qubits, src_mat);
		break;

	default:
	{
		//QR decompose
		std::vector<SingleGateUnit> cir_units;
		general_scheme(src_mat, cir_units);
		circuit_insert(qubits, cir_units, output_circuit);
	}
		break;
	}

	return output_circuit;
}

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


#include "Angle.hpp"
#include <boost/format.hpp>
#include <base/Eigen.hpp>

namespace base {

Angle Angle::vectorToVector(const Vector3d& a, const Vector3d& b)
{
    double dot = a.dot(b);
    double norm = a.norm() * b.norm();
    return fromRad(acos(dot / norm));
}

Angle Angle::vectorToVector(const Vector3d& a, const Vector3d& b, const Vector3d& positive)
{
    double cos = a.dot(b) / (a.norm() * b.norm());

    bool is_positive = (a.cross(b).dot(positive) > 0);
    if (is_positive)
        return fromRad(acos(cos));
    else
        return fromRad(-acos(cos));
}

std::ostream& operator << (std::ostream& os, Angle angle)
{
    os << angle.getRad() << boost::format("[%3.1fdeg]") % angle.getDeg();
    return os;
}

AngleSegment::AngleSegment(): width(0), startRad(0), endRad(0)
{
}

AngleSegment::AngleSegment(const Angle &start, double _width): width(_width), startRad(start.getRad()), endRad(startRad + width)
{
    if(width < 0)
        throw std::runtime_error("Error got segment with negative width");
}

bool AngleSegment::isInside(const Angle& angle) const
{
    double angleRad = angle.getRad();
    if(angleRad < startRad)
        angleRad += 2*M_PI;

    if(angleRad <= endRad) //startRad <= angleRad &&
        return true;

    return false;
}

bool AngleSegment::isInside(const AngleSegment& segment) const
{
    double otherStart = segment.startRad;
    if(otherStart < startRad)
        otherStart += 2*M_PI;

    double otherEnd = otherStart + segment.width;

    if(otherEnd <= endRad)
        return true;

    return false;
}

std::vector< AngleSegment > AngleSegment::getIntersections(const AngleSegment& b) const
{
    std::vector<AngleSegment> ret;
    //special case, this segment is a whole circle
    if(width >= 2*M_PI)
    {
        ret.push_back(b);
        return ret;
    }

    //special case, other segment is a whole circle
    if(b.width >= 2*M_PI)
    {
        ret.push_back(*this);
        return ret;
    }

    double startA = startRad;
    double startB = b.startRad;
    double widthA = width;
    double widthB = b.width;

    //make A the smaller angle
    if(startA > startB)
    {
        std::swap(startA, startB);
        std::swap(widthA, widthB);
    }
    double endA = startA + widthA;
    double endB = startB + widthB;

    //test if segemnts do not intersect at all
    if(endA < startB)
    {
        //wrap case
        if(endB > M_PI)
        {
            //check if segments intersect after wrap correction
            if(startA < endB - 2*M_PI)
            {
                //this means the start of A is inside of B
                //drop first part of B and realign it to -M_PI
                //also switch A and B as B is now the 'lower' one
                double newWidthA = widthB - (M_PI - startB);
                startB = startA;
                widthB = widthA;
                startA = - M_PI;
                widthA = newWidthA;
                endA = startA + widthA;
                endB = startB + widthB;
                //no return, still need
                //to check for intersection
            }
            else
                //no intersection
                return ret;
        } else
                //no intersection
            return ret;
    }

    //normal case, no wrap around
    double newStart = startB;
    double newEnd = 0;

    if(endA < endB)
    {
        newEnd = endA;
    }
    else
    {
        newEnd = endB;
    }

    double newWidth = newEnd - newStart;

    //filter invalid segments
    if(newWidth > 1e-10)
        ret.push_back(AngleSegment(Angle::fromRad(newStart), newWidth));

    newStart = endB - 2*M_PI;
    if(newStart > startA)
    {
        newWidth = newStart - startA;
        //filter invalid segments
        if(newWidth > 1e-10)
            ret.push_back(AngleSegment(Angle::fromRad(startA), newWidth));
    }

    return ret;
}

Angle AngleSegment::getStart() const
{
    return Angle::fromRad(startRad);
}

Angle AngleSegment::getEnd() const
{
    return Angle::fromRad(endRad);
}

std::ostream& operator << (std::ostream& os, AngleSegment seg)
{
    os << " Segmend start " << seg.startRad/M_PI *180.0 << " end  " << seg.endRad/M_PI * 180.0 << " width " << seg.width /M_PI * 180.0;
    return os;
}

} //end namespace base


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * 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.
 ********************************************************************************/

#ifndef RW_MATH_QUATERNION_HPP
#define RW_MATH_QUATERNION_HPP

/**
 * @file Quaternion.hpp
 */

#if !defined(SWIG)
#include <rw/common/Serializable.hpp>
#include <rw/math/Rotation3D.hpp>
#include <rw/math/Rotation3DVector.hpp>

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

namespace rw { namespace math {

    /** @addtogroup math */
    /*@{*/

    /**
     * @brief A Quaternion @f$ \mathbf{q}\in \mathbb{R}^4 @f$ a complex
     * number used to describe rotations in 3-dimensional space.
     * @f$ q_w+{\bf i}\ q_x+ {\bf j} q_y+ {\bf k}\ q_z @f$
     *
     * Quaternions can be added and multiplied in a similar way as usual
     * algebraic numbers. Though there are differences. Quaternion
     * multiplication is not commutative which means
     * \f$ Q\cdot P \neq P\cdot Q \f$
     */
    template< class T = double > class Quaternion : public rw::math::Rotation3DVector< T >
    {
      private:
        typedef Eigen::Quaternion< T > EigenQuaternion;

      public:

        //! Value type.
        typedef T value_type;

        /**
         * @brief constuct Quaterinion of {0,0,0,1}
         */
        Quaternion () : _q (0, 0, 0, 1) {}

        /**
         * @brief Creates a Quaternion
         * @param qx [in] @f$ q_x @f$
         * @param qy [in] @f$ q_y @f$
         * @param qz [in] @f$ q_z @f$
         * @param qw  [in] @f$ q_w @f$
         */
        Quaternion (T qx, T qy, T qz, T qw) : _q (qw, qx, qy, qz) {}

        /**
         * @brief Creates a Quaternion from another Quaternion
         * @param quat [in] Quaternion
         */
        Quaternion (const Quaternion< T >& quat) : _q (quat._q) {}

        /**
         * @brief Creates a Quaternion from another Rotation3DVector type
         * @param rot [in] The Rotation3DVector type
         */
        Quaternion (const rw::math::Rotation3DVector< T >& rot)
        {
            setRotation (rot.toRotation3D ());
        }

        /**
         * @brief Extracts a Quaternion from Rotation matrix using
         * setRotation(const Rotation3D<R>& rot)
         * @param rot [in] A 3x3 rotation matrix @f$ \mathbf{rot} @f$
         */
        Quaternion (const rw::math::Rotation3D< T >& rot) { setRotation (rot); }

        /**
         * @brief Creates a Quaternion from a Eigen quaternion
         * @param r [in] a boost quaternion
         */
        Quaternion (const Eigen::Quaternion< T >& r) : _q (r) {}

        /**
         * @brief Creates a Quaternion from vector_expression
         *
         * @param r [in] an Eigen Vector
         */
        template< class R >
        explicit Quaternion (const Eigen::MatrixBase< R >& r) :
            _q (r.row (3) (0), r.row (0) (0), r.row (1) (0), r.row (2) (0))
        {}

        // ###################################################
        // #                Acces Operators                  #
        // ###################################################

        /**
         * @brief get method for the x component
         * @return the x component of the quaternion
         */
        inline T getQx () const { return _q.x (); }

        /**
         * @brief get method for the y component
         * @return the y component of the quaternion
         */
        inline T getQy () const { return _q.y (); }

        /**
         * @brief get method for the z component
         * @return the z component of the quaternion
         */
        inline T getQz () const { return _q.z (); }

        /**
         * @brief get method for the w component
         * @return the w component of the quaternion
         */
        inline T getQw () const { return _q.w (); }

#if !defined(SWIG)
        /**
         * @brief Returns reference to Quaternion element
         * @param i [in] index in the quaternion \f$i\in \{0,1,2,3\} \f$
         * @return const reference to element
         */
        inline T operator() (size_t i) const
        {
            switch (i) {
                case 0: return _q.x ();
                case 1: return _q.y ();
                case 2: return _q.z ();
                case 3: return _q.w ();
                default: assert (0); return _q.x ();
            }
        }

        /**
         * @brief Returns reference to Quaternion element
         * @param i [in] index in the quaternion \f$i\in \{0,1,2,3\} \f$
         * @return reference to element
         */
        inline T& operator() (size_t i)
        {
            switch (i) {
                case 0: return _q.x ();
                case 1: return _q.y ();
                case 2: return _q.z ();
                case 3: return _q.w ();
                default: assert (0); return _q.x ();
            }
        }

        /**
         * @brief Returns reference to Quaternion element
         * @param i [in] index in the quaternion \f$i\in \{0,1,2,3\} \f$
         * @return reference to element
         */
        inline T& operator[] (size_t i)
        {
            switch (i) {
                case 0: return _q.x ();
                case 1: return _q.y ();
                case 2: return _q.z ();
                case 3: return _q.w ();
                default: assert (0); return _q.x ();
            }
        }

        /**
         * @brief Returns reference to Quaternion element
         * @param i [in] index in the quaternion \f$i\in \{0,1,2,3\} \f$
         * @return reference to element
         */
        inline T operator[] (size_t i) const
        {
            switch (i) {
                case 0: return _q.x ();
                case 1: return _q.y ();
                case 2: return _q.z ();
                case 3: return _q.w ();
                default: assert (0); return _q.x ();
            }
        }
#else
        ARRAYOPERATOR (T);
#endif
#if !defined(SWIGJAVA)

        /**
         * @brief Calculates the @f$ 3\times 3 @f$ Rotation matrix
         *
         * @return A 3x3 rotation matrix @f$ \mathbf{rot} @f$
         * @f$
         * \mathbf{rot} =
         *  \left[
         *   \begin{array}{ccc}
         *      1-2(q_y^2-q_z^2) & 2(q_x\ q_y+q_z\ q_w)& 2(q_x\ q_z-q_y\ q_w) \\
         *      2(q_x\ q_y-q_z\ q_w) & 1-2(q_x^2-q_z^2) & 2(q_y\ q_z+q_x\ q_w)\\
         *      2(q_x\ q_z+q_y\ q_w) & 2(q_y\ q_z-q_x\ q_z) & 1-2(q_x^2-q_y^2)
         *    \end{array}
         *  \right]
         * @f$
         *
         */
#endif
        inline const rw::math::Rotation3D< T > toRotation3D () const
        {
            const T qx = _q.x ();
            const T qy = _q.y ();
            const T qz = _q.z ();
            const T qw = _q.w ();

            return rw::math::Rotation3D< T > (1 - 2 * qy * qy - 2 * qz * qz,
                                              2 * (qx * qy - qz * qw),
                                              2 * (qx * qz + qy * qw),
                                              2 * (qx * qy + qz * qw),
                                              1 - 2 * qx * qx - 2 * qz * qz,
                                              2 * (qy * qz - qx * qw),
                                              2 * (qx * qz - qy * qw),
                                              2 * (qy * qz + qx * qw),
                                              1 - 2 * qx * qx - 2 * qy * qy);
        }

        /** @brief Converts a Rotation3D to a Quaternion and saves the Quaternion
         * in this.
         *
         * @param rot [in] A 3x3 rotation matrix @f$ \mathbf{R} @f$
         *
         * @f$
         * \begin{array}{c}
         * q_x\\ q_y\\ q_z\\ q_w
         * \end{array}
         * =
         *  \left[
         *   \begin{array}{c}
         *      \\
         *      \\
         *
         *    \end{array}
         *  \right]
         * @f$
         *
         * The conversion method is proposed by Henrik Gordon Petersen. The switching between
         * different cases occur well before numerical instabilities, hence the solution should be
         * more robust, than many of the methods proposed elsewhere.
         *
         */
        template< class R > void setRotation (const rw::math::Rotation3D< R >& rot)
        {
            // The method
            const T min  = (T) (-0.9);
            const T min1 = (T) (min / 3.0);

            const T tr = rot (0, 0) + rot (1, 1) + rot (2, 2);

            if (tr > min) {
                const T s = static_cast< T > (0.5) / static_cast< T > (sqrt (tr + 1.0));
                _q.w ()   = static_cast< T > (0.25) / s;
                _q.x ()   = static_cast< T > (rot (2, 1) - rot (1, 2)) * s;
                _q.y ()   = static_cast< T > (rot (0, 2) - rot (2, 0)) * s;
                _q.z ()   = static_cast< T > (rot (1, 0) - rot (0, 1)) * s;
            }
            else {
                if (rot (0, 0) > min1) {
                    const T sa =
                        static_cast< T > (sqrt (rot (0, 0) - rot (1, 1) - rot (2, 2) + 1.0));
                    _q.x ()   = static_cast< T > (0.5) * sa;
                    const T s = static_cast< T > (0.25) / _q.x ();
                    _q.y ()   = static_cast< T > (rot (0, 1) + rot (1, 0)) * s;
                    _q.z ()   = static_cast< T > (rot (0, 2) + rot (2, 0)) * s;
                    _q.w ()   = static_cast< T > (rot (2, 1) - rot (1, 2)) * s;
                }
                else if (rot (1, 1) > min1) {
                    const T sb = static_cast< T > (sqrt (rot (1, 1) - rot (2, 2) - rot (0, 0) + 1));
                    _q.y ()    = static_cast< T > (0.5) * sb;

                    const T s = static_cast< T > (0.25) / _q.y ();
                    _q.x ()   = static_cast< T > (rot (0, 1) + rot (1, 0)) * s;
                    _q.z ()   = static_cast< T > (rot (1, 2) + rot (2, 1)) * s;
                    _q.w ()   = static_cast< T > (rot (0, 2) - rot (2, 0)) * s;
                }
                else {
                    const T sc = static_cast< T > (sqrt (rot (2, 2) - rot (0, 0) - rot (1, 1) + 1));
                    _q.z ()    = static_cast< T > (0.5) * sc;

                    const T s = static_cast< T > (0.25) / _q.z ();
                    _q.x ()   = static_cast< T > (rot (0, 2) + rot (2, 0)) * s;
                    _q.y ()   = static_cast< T > (rot (1, 2) + rot (2, 1)) * s;
                    _q.w ()   = static_cast< T > (rot (1, 0) - rot (0, 1)) * s;
                }
            }
        }

        /**
         * @brief The dimension of the quaternion (i.e. 4).
         * This method is provided to help support generic algorithms using
         * size() and operator[].
         */
        size_t size () const { return 4; }

        /**
         * @brief Convert to an Eigen Quaternion.
         * @return Eigen Quaternion representation.
         */
        Eigen::Quaternion< T >& e () { return _q; }

        //! @copydoc e()
        const Eigen::Quaternion< T >& e () const { return _q; }

        /**
         * @brief convert to Eigen Vector
         * @return eigen Vector of quaternion
         */
        Eigen::Matrix< T, 4, 1 > toEigenVector () const
        {
            return Eigen::Matrix< T, 4, 1 > (_q.x (), _q.y (), _q.z (), _q.w ());
        }

        // ###################################################
        // #                 Math Operators                  #
        // ###################################################

        // ############ Quaternion Operations

        /**
         * @brief Unary minus.
         */
        Quaternion< T > operator- () const
        {
            return Quaternion (-_q.x (), -_q.y (), -_q.z (), -_q.w ());
        }

        /**
         * @brief Unary plus.
         */
        Quaternion< T > operator+ () const { return Quaternion (*this); }

        /**
         * @brief Subtraction.
         */
        inline const Quaternion< T > operator- (const Quaternion< T >& v)
        {
            return Quaternion< T > (
                (*this) (0) - v (0), (*this) (1) - v (1), (*this) (2) - v (2), (*this) (3) - v (3));
        }

        /**
         * @brief Multiply-from operator
         */
        inline Quaternion< T > operator* (const Quaternion< T >& r) const
        {
            Quaternion q = Quaternion (_q * r.e ());
            return q;
        }

        /**
           @brief Addition of two quaternions
         */
        inline const Quaternion< T > operator+ (const Quaternion< T >& v) const
        {
            return Quaternion< T > (
                (*this) (0) + v (0), (*this) (1) + v (1), (*this) (2) + v (2), (*this) (3) + v (3));
        }

        // ############ Scalar Operations

        /**
         * @brief Scalar multiplication.
         */
        inline const Quaternion< T > operator* (T s) const
        {
            return Quaternion< T > (_q.x () * s, _q.y () * s, _q.z () * s, _q.w () * s);
        }
#if !defined(SWIGPYTHON)
        /**
         * @brief Scalar multiplication.
         */
        inline friend const Quaternion< T > operator* (T s, const Quaternion< T >& v)
        {
            return v * s;
        }
#endif
        /**
         * @brief element whise division
         * @param lhs [in] the scalar to devide with
         * @return the result of elementwise devision
         */
        Quaternion< T > elemDivide (const T& lhs) const;

        // ############ Math Operations

        /**
         * @brief get length of quaternion
         * @f$ \sqrt{q_x^2+q_y^2+q_z^2+q_w^2} @f$
         * @return the length og this quaternion
         */
        inline T getLength () const { return _q.norm (); }

        /**
         * @brief get squared length of quaternion
         * @f$ q_x^2+q_y^2+q_z^2+q_w^2 @f$
         * @return the length og this quaternion
         */
        inline T getLengthSquared () const { return _q.squaredNorm (); }

        /**
         * @brief normalizes this quaternion so that
         * @f$ normalze(Q)=\frac{Q}{\sqrt{q_x^2+q_y^2+q_z^2+q_w^2}} @f$
         */
        inline void normalize () { _q.normalize (); };

        /**
         * @brief Calculates a slerp interpolation between \b this and \b v.
         *
         * The slerp interpolation ensures a constant velocity across the interpolation.
         * For \f$ t=0\f$ the result is \b this and for \f$ t=1\f$ it is \b v.
         *
         * @note Algorithm and implementation is thanks to euclideanspace.com
         */
        inline const Quaternion< T > slerp (const Quaternion< T >& v, const T t) const
        {
            return Quaternion (_q.slerp (t, v.e ()));
        }

        /*
         * @brief this will return the exponential of this quaternion \f$ e^Quaternion \f$
         * @return the exponential of this quaternion
         */
        Quaternion< T > exp () const;

        /**
         * @brief Calculate the inverse Quaternion
         * @return the inverse quaternion
         */
        Quaternion< T > inverse () const;

        /**
         * @brief calculates the natural logerithm of this quaternion
         * @return natural logetihm
         */
        Quaternion< T > ln () const;

        /**
         * @brief calculates the quaternion lifted to the power of \b power
         * @param power [in] the power the quaternion is lifted to
         * @return \f$ Quaternion^power \f$
         */
        Quaternion< T > pow (double power) const;

        // ###################################################
        // #             assignement Operators               #
        // ###################################################

        /**
         * @brief copy a boost quaternion to this Quaternion
         * @param r [in] - boost quaternion
         */
        inline void operator= (const Eigen::Quaternion< T >& r) { _q = r; }

        /**
           @brief Scalar multiplication.
         */
        inline const Quaternion< T > operator*= (T s)
        {
            _q.x () *= s;
            _q.y () *= s;
            _q.z () *= s;
            _q.w () *= s;
            return *this;
        }

        /**
         * @brief Multiply with operator
         */
        inline const Quaternion< T > operator*= (const Quaternion< T >& r)
        {
            *this = (*this) * r;
            return *this;
        }

        /**
         *@brief Add-to operator
         */
        inline const Quaternion< T > operator+= (const Quaternion< T >& r)
        {
            _q.x () += r (0);
            _q.y () += r (1);
            _q.z () += r (2);
            _q.w () += r (3);
            return *this;
        }

        /**
         * @brief Subtract-from operator
         */
        inline const Quaternion< T > operator-= (const Quaternion< T >& r)
        {
            _q.x () -= r (0);
            _q.y () -= r (1);
            _q.z () -= r (2);
            _q.w () -= r (3);
            return *this;
        }

        /**
         * @brief copyfrom rotaion matrix, same as setRotation.
         * @param rhs [in] the rotation that will be copyed
         */
        Quaternion< T >& operator= (const rw::math::Rotation3D<>& rhs)
        {
            this->setRotation (rhs);
            return (*this);
        }

        // ###################################################
        // #              Comparison Operators               #
        // ###################################################

        /**
         * @brief Comparison (equals) operator
         */
        inline bool operator== (const Quaternion< T >& r) const
        {
            return (*this) (0) == r (0) && (*this) (1) == r (1) && (*this) (2) == r (2) &&
                   (*this) (3) == r (3);
        }

        /**
         * @brief Comparison (not equals) operator
         */
        inline bool operator!= (const Quaternion< T >& r) const { return !((*this) == r); }

#if defined(SWIG)
        TOSTRING ();
#endif
      private:
        Eigen::Quaternion< T > _q;
    };

    /**
       @brief Streaming operator.

       @relates Quaternion
    */
    template< class T > std::ostream& operator<< (std::ostream& out, const Quaternion< T >& v)
    {
        return out << "Quaternion {" << v (0) << ", " << v (1) << ", " << v (2) << ", " << v (3)
                   << "}";
    }

    /**
     * @brief calculates the natural logerithm of this quaternion
     * @param q [in] the quaternion being operated on
     * @return natural logetihm
     */
    template< class T > Quaternion< T > ln (const Quaternion< T >& q) { return q.ln (); }

    /**
     * @brief this will return the exponential of this quaternion \f$ e^Quaternion \f$
     * @param q [in] the quaternion being operated on
     * @return the exponential of this quaternion
     */
    template< class T > Quaternion< T > exp (const Quaternion< T >& q) { return q.exp (); }

    /**
     * @brief Calculate the inverse Quaternion
     * @param q [in] the quaternion being operated on
     * @return the inverse quaternion
     */
    template< class T > Quaternion< T > inverse (const Quaternion< T >& q) { return q.inverse (); }

    /**
     * @brief calculates the quaternion lifted to the power of \b power
     * @param q [in] the quaternion being operated on
     * @param power [in] the power the quaternion is lifted to
     * @return \f$ Quaternion^power \f$
     */
    template< class T > Quaternion< T > pow (const Quaternion< T >& q, double power)
    {
        return q.pow (power);
    }

    /**
     * @brief Casts Quaternion<T> to Quaternion<Q>
     * @param quaternion [in] Quarternion with type T
     * @return Quaternion with type Q
     */
    template< class Q, class T >
    inline const Quaternion< Q > cast (const Quaternion< T >& quaternion)
    {
        return Quaternion< Q > (static_cast< Q > (quaternion (0)),
                                static_cast< Q > (quaternion (1)),
                                static_cast< Q > (quaternion (2)),
                                static_cast< Q > (quaternion (3)));
    }
#if !defined(SWIG)
    extern template class rw::math::Quaternion< double >;
    extern template class rw::math::Quaternion< float >;
#else

#if SWIG_VERSION < 0x040000
    SWIG_DECLARE_TEMPLATE (Quaternion_d, rw::math::Quaternion< double >);
    ADD_DEFINITION (Quaternion_d, Quaternion)
#else
    SWIG_DECLARE_TEMPLATE (Quaternion, rw::math::Quaternion< double >);
#endif
    SWIG_DECLARE_TEMPLATE (Quaternion_f, rw::math::Quaternion< float >);
#endif
    using Quaterniond = Quaternion< double >;
    using Quaternionf = Quaternion< float >;

    /*@}*/
}}    // namespace rw::math

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Quaternion
         */
        template<>
        void write (const rw::math::Quaternion< double >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Quaternion
         */
        template<>
        void write (const rw::math::Quaternion< float >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Quaternion
         */
        template<>
        void read (rw::math::Quaternion< double >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Quaternion
         */
        template<>
        void read (rw::math::Quaternion< float >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);
    }    // namespace serialization
}}       // namespace rw::common

#endif    // end include guard


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

// @sect3{Include files}

// 像往常一样，第一个任务是包括这些著名的deal.II库文件和一些C++头文件的功能。

#include <deal.II/base/discrete_time.h>
#include <deal.II/base/function.h>
#include <deal.II/base/quadrature_lib.h>

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_out.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/lac/affine_constraints.h>
#include <deal.II/lac/sparse_direct.h>

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

#include <fstream>
#include <iostream>
#include <cmath>
#include <map>

// 这是唯一一个新的包含文件：它包括所有的Runge-Kutta方法。

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

// 接下来的步骤与之前所有的教程程序一样。我们把所有的东西放到一个自己的命名空间中，然后把deal.II的类和函数导入其中。

namespace Step52
{
  using namespace dealii;
// @sect3{The Diffusion class}

// 下一块是主类的声明。这个类中的大部分函数并不新鲜，在以前的教程中已经解释过了。唯一有趣的函数是  evaluate_diffusion()  和  id_minus_tau_J_inverse(). evaluate_diffusion()  评估扩散方程，  $M^{-1}(f(t,y))$  ，在一个给定的时间和一个给定的  $y$  。  id_minus_tau_J_inverse()  在给定的时间和给定的 $\tau$ 和 $y$ 下，评估 $\left(I-\tau M^{-1} \frac{\partial f(t,y)}{\partial y}\right)^{-1}$ 或类似的 $\left(M-\tau \frac{\partial f}{\partial y}\right)^{-1} M$ 。当使用隐式方法时，就需要这个函数。

  class Diffusion
  {
  public:
    Diffusion();

    void run();

  private:
    void setup_system();

    void assemble_system();

    double get_source(const double time, const Point<2> &point) const;

    Vector<double> evaluate_diffusion(const double          time,
                                      const Vector<double> &y) const;

    Vector<double> id_minus_tau_J_inverse(const double          time,
                                          const double          tau,
                                          const Vector<double> &y);

    void output_results(const double                     time,
                        const unsigned int               time_step,
                        TimeStepping::runge_kutta_method method) const;

// 接下来的三个函数分别是显式方法、隐式方法和嵌入式显式方法的驱动。嵌入显式方法的驱动函数返回执行的步数，鉴于它只接受作为参数传递的时间步数作为提示，但内部计算了最佳时间步数本身。

    void explicit_method(const TimeStepping::runge_kutta_method method,
                         const unsigned int                     n_time_steps,
                         const double                           initial_time,
                         const double                           final_time);

    void implicit_method(const TimeStepping::runge_kutta_method method,
                         const unsigned int                     n_time_steps,
                         const double                           initial_time,
                         const double                           final_time);

    unsigned int
    embedded_explicit_method(const TimeStepping::runge_kutta_method method,
                             const unsigned int n_time_steps,
                             const double       initial_time,
                             const double       final_time);

    const unsigned int fe_degree;

    const double diffusion_coefficient;
    const double absorption_cross_section;

    Triangulation<2> triangulation;

    const FE_Q<2> fe;

    DoFHandler<2> dof_handler;

    AffineConstraints<double> constraint_matrix;

    SparsityPattern sparsity_pattern;

    SparseMatrix<double> system_matrix;
    SparseMatrix<double> mass_matrix;
    SparseMatrix<double> mass_minus_tau_Jacobian;

    SparseDirectUMFPACK inverse_mass_matrix;

    Vector<double> solution;
  };

// 我们选择二次方有限元，并初始化参数。

  Diffusion::Diffusion()
    : fe_degree(2)
    , diffusion_coefficient(1. / 30.)
    , absorption_cross_section(1.)
    , fe(fe_degree)
    , dof_handler(triangulation)
  {}

// 现在，我们创建约束矩阵和稀疏模式。然后，我们初始化这些矩阵和求解向量。

  void Diffusion::setup_system()
  {
    dof_handler.distribute_dofs(fe);

    VectorTools::interpolate_boundary_values(dof_handler,
                                             1,
                                             Functions::ZeroFunction<2>(),
                                             constraint_matrix);
    constraint_matrix.close();

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

    system_matrix.reinit(sparsity_pattern);
    mass_matrix.reinit(sparsity_pattern);
    mass_minus_tau_Jacobian.reinit(sparsity_pattern);
    solution.reinit(dof_handler.n_dofs());
  }

//  @sect4{Diffusion::assemble_system}  在这个函数中，我们计算  $-\int D \nabla b_i \cdot \nabla b_j d\boldsymbol{r} - \int \Sigma_a b_i b_j d\boldsymbol{r}$  和质量矩阵  $\int b_i b_j d\boldsymbol{r}$  。然后使用直接求解器对质量矩阵进行反演；然后 inverse_mass_matrix 变量将存储质量矩阵的反值，这样 $M^{-1}$ 就可以使用该对象的 vmult() 函数应用于一个矢量。在内部，UMFPACK并没有真正存储矩阵的逆，而是存储它的LU因子；应用矩阵的逆相当于用这两个因子做一次正解和一次逆解，这与应用矩阵的显式逆具有相同的复杂性）。

  void Diffusion::assemble_system()
  {
    system_matrix = 0.;
    mass_matrix   = 0.;

    const QGauss<2> quadrature_formula(fe_degree + 1);

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

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

    FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
    FullMatrix<double> cell_mass_matrix(dofs_per_cell, dofs_per_cell);

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

    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        cell_matrix      = 0.;
        cell_mass_matrix = 0.;

        fe_values.reinit(cell);

        for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
          for (unsigned int i = 0; i < dofs_per_cell; ++i)
            for (unsigned int j = 0; j < dofs_per_cell; ++j)
              {
                cell_matrix(i, j) +=
                  ((-diffusion_coefficient *                // (-D
                      fe_values.shape_grad(i, q_point) *    //  * grad phi_i
                      fe_values.shape_grad(j, q_point)      //  * grad phi_j
                    - absorption_cross_section *            //  -Sigma
                        fe_values.shape_value(i, q_point) * //  * phi_i
                        fe_values.shape_value(j, q_point))  //  * phi_j)
                   * fe_values.JxW(q_point));               // * dx
                cell_mass_matrix(i, j) += fe_values.shape_value(i, q_point) *
                                          fe_values.shape_value(j, q_point) *
                                          fe_values.JxW(q_point);
              }

        cell->get_dof_indices(local_dof_indices);

        constraint_matrix.distribute_local_to_global(cell_matrix,
                                                     local_dof_indices,
                                                     system_matrix);
        constraint_matrix.distribute_local_to_global(cell_mass_matrix,
                                                     local_dof_indices,
                                                     mass_matrix);
      }

    inverse_mass_matrix.initialize(mass_matrix);
  }

//  @sect4{Diffusion::get_source}

// 在这个函数中，计算出特定时间和特定点的方程的源项。

  double Diffusion::get_source(const double time, const Point<2> &point) const
  {
    const double intensity = 10.;
    const double frequency = numbers::PI / 10.;
    const double b         = 5.;
    const double x         = point(0);

    return intensity *
           (frequency * std::cos(frequency * time) * (b * x - x * x) +
            std::sin(frequency * time) *
              (absorption_cross_section * (b * x - x * x) +
               2. * diffusion_coefficient));
  }

//  @sect4{Diffusion::evaluate_diffusion}

// 接下来，我们在给定的时间  $t$  和给定的矢量  $y$  评价扩散方程的弱形式。换句话说，正如介绍中所述，我们评估  $M^{-1}(-{\cal D}y - {\cal A}y + {\cal S})$  。为此，我们必须将矩阵 $-{\cal D} - {\cal A}$ （之前计算并存储在变量 system_matrix 中）应用于 $y$ ，然后添加源项，我们像通常那样进行积分。(如果你想节省几行代码，或者想利用并行积分的优势，可以用 VectorTools::create_right_hand_side() 来进行积分。) 然后将结果乘以 $M^{-1}$  。

  Vector<double> Diffusion::evaluate_diffusion(const double          time,
                                               const Vector<double> &y) const
  {
    Vector<double> tmp(dof_handler.n_dofs());
    tmp = 0.;
    system_matrix.vmult(tmp, y);

    const QGauss<2> quadrature_formula(fe_degree + 1);

    FEValues<2> fe_values(fe,
                          quadrature_formula,
                          update_values | 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();

    Vector<double> cell_source(dofs_per_cell);

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

    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        cell_source = 0.;

        fe_values.reinit(cell);

        for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
          {
            const double source =
              get_source(time, fe_values.quadrature_point(q_point));
            for (unsigned int i = 0; i < dofs_per_cell; ++i)
              cell_source(i) += fe_values.shape_value(i, q_point) * // phi_i(x)
                                source *                            // * S(x)
                                fe_values.JxW(q_point);             // * dx
          }

        cell->get_dof_indices(local_dof_indices);

        constraint_matrix.distribute_local_to_global(cell_source,
                                                     local_dof_indices,
                                                     tmp);
      }

    Vector<double> value(dof_handler.n_dofs());
    inverse_mass_matrix.vmult(value, tmp);

    return value;
  }
// @sect4{Diffusion::id_minus_tau_J_inverse}

// 我们计算  $\left(M-\tau \frac{\partial f}{\partial y}\right)^{-1} M$  。这要分几个步骤进行。

// - 计算  $M-\tau \frac{\partial f}{\partial y}$  。

// - 反转矩阵，得到  $\left(M-\tau \frac{\partial f} {\partial y}\right)^{-1}$  。

// --计算 $tmp=My$ 。

// --计算 $z=\left(M-\tau \frac{\partial f}{\partial y}\right)^{-1} tmp =  \left(M-\tau \frac{\partial f}{\partial y}\right)^{-1} My$ 。

// - 返回z。

  Vector<double> Diffusion::id_minus_tau_J_inverse(const double /*time*/,
                                                   const double          tau,
                                                   const Vector<double> &y)
  {
    SparseDirectUMFPACK inverse_mass_minus_tau_Jacobian;

    mass_minus_tau_Jacobian.copy_from(mass_matrix);
    mass_minus_tau_Jacobian.add(-tau, system_matrix);

    inverse_mass_minus_tau_Jacobian.initialize(mass_minus_tau_Jacobian);

    Vector<double> tmp(dof_handler.n_dofs());
    mass_matrix.vmult(tmp, y);

    Vector<double> result(y);
    inverse_mass_minus_tau_Jacobian.vmult(result, tmp);

    return result;
  }

//  @sect4{Diffusion::output_results}

// 下面的函数将解决方案以vtu文件的形式输出，并以时间步长和时间步长方法的名称为索引。当然，所有时间步长方法的（精确）结果应该是一样的，但这里的输出至少可以让我们对它们进行比较。

  void Diffusion::output_results(const double                     time,
                                 const unsigned int               time_step,
                                 TimeStepping::runge_kutta_method method) const
  {
    std::string method_name;

    switch (method)
      {
        case TimeStepping::FORWARD_EULER:
          {
            method_name = "forward_euler";
            break;
          }
        case TimeStepping::RK_THIRD_ORDER:
          {
            method_name = "rk3";
            break;
          }
        case TimeStepping::RK_CLASSIC_FOURTH_ORDER:
          {
            method_name = "rk4";
            break;
          }
        case TimeStepping::BACKWARD_EULER:
          {
            method_name = "backward_euler";
            break;
          }
        case TimeStepping::IMPLICIT_MIDPOINT:
          {
            method_name = "implicit_midpoint";
            break;
          }
        case TimeStepping::SDIRK_TWO_STAGES:
          {
            method_name = "sdirk";
            break;
          }
        case TimeStepping::HEUN_EULER:
          {
            method_name = "heun_euler";
            break;
          }
        case TimeStepping::BOGACKI_SHAMPINE:
          {
            method_name = "bocacki_shampine";
            break;
          }
        case TimeStepping::DOPRI:
          {
            method_name = "dopri";
            break;
          }
        case TimeStepping::FEHLBERG:
          {
            method_name = "fehlberg";
            break;
          }
        case TimeStepping::CASH_KARP:
          {
            method_name = "cash_karp";
            break;
          }
        default:
          {
            break;
          }
      }

    DataOut<2> data_out;

    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution, "solution");

    data_out.build_patches();

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

    const std::string filename = "solution_" + method_name + "-" +
                                 Utilities::int_to_string(time_step, 3) +
                                 ".vtu";
    std::ofstream output(filename);
    data_out.write_vtu(output);

    static std::vector<std::pair<double, std::string>> times_and_names;

    static std::string method_name_prev = "";
    static std::string pvd_filename;
    if (method_name_prev != method_name)
      {
        times_and_names.clear();
        method_name_prev = method_name;
        pvd_filename     = "solution_" + method_name + ".pvd";
      }
    times_and_names.emplace_back(time, filename);
    std::ofstream pvd_output(pvd_filename);
    DataOutBase::write_pvd_record(pvd_output, times_and_names);
  }
// @sect4{Diffusion::explicit_method}

// 这个函数是所有显式方法的驱动。在顶部，它初始化了时间步长和解决方案（通过将其设置为零，然后确保边界值和悬挂节点约束得到尊重；当然，对于我们在这里使用的网格，悬挂节点约束实际上不是一个问题）。然后调用 evolve_one_time_step ，执行一个时间步骤。时间是通过DiscreteTime对象来存储和增加的。

// 对于显式方法， evolve_one_time_step 需要评估 $M^{-1}(f(t,y))$ ，也就是说，它需要 evaluate_diffusion  。因为 evaluate_diffusion 是一个成员函数，它需要被绑定到 this 。在每个进化步骤之后，我们再次应用正确的边界值和悬挂节点约束。

// 最后，每隔10个时间步骤就会输出解决方案。

  void Diffusion::explicit_method(const TimeStepping::runge_kutta_method method,
                                  const unsigned int n_time_steps,
                                  const double       initial_time,
                                  const double       final_time)
  {
    const double time_step =
      (final_time - initial_time) / static_cast<double>(n_time_steps);

    solution = 0.;
    constraint_matrix.distribute(solution);

    TimeStepping::ExplicitRungeKutta<Vector<double>> explicit_runge_kutta(
      method);
    output_results(initial_time, 0, method);
    DiscreteTime time(initial_time, final_time, time_step);
    while (time.is_at_end() == false)
      {
        explicit_runge_kutta.evolve_one_time_step(
          [this](const double time, const Vector<double> &y) {
            return this->evaluate_diffusion(time, y);
          },
          time.get_current_time(),
          time.get_next_step_size(),
          solution);
        time.advance_time();

        constraint_matrix.distribute(solution);

        if (time.get_step_number() % 10 == 0)
          output_results(time.get_current_time(),
                         time.get_step_number(),
                         method);
      }
  }

//  @sect4{Diffusion::implicit_method}  这个函数等同于 explicit_method ，但用于隐式方法。当使用隐式方法时，我们需要评估 $M^{-1}(f(t,y))$ 和 $\left(I-\tau M^{-1} \frac{\partial f(t,y)}{\partial y}\right)^{-1}$ ，为此我们使用之前介绍的两个成员函数。

  void Diffusion::implicit_method(const TimeStepping::runge_kutta_method method,
                                  const unsigned int n_time_steps,
                                  const double       initial_time,
                                  const double       final_time)
  {
    const double time_step =
      (final_time - initial_time) / static_cast<double>(n_time_steps);

    solution = 0.;
    constraint_matrix.distribute(solution);

    TimeStepping::ImplicitRungeKutta<Vector<double>> implicit_runge_kutta(
      method);
    output_results(initial_time, 0, method);
    DiscreteTime time(initial_time, final_time, time_step);
    while (time.is_at_end() == false)
      {
        implicit_runge_kutta.evolve_one_time_step(
          [this](const double time, const Vector<double> &y) {
            return this->evaluate_diffusion(time, y);
          },
          [this](const double time, const double tau, const Vector<double> &y) {
            return this->id_minus_tau_J_inverse(time, tau, y);
          },
          time.get_current_time(),
          time.get_next_step_size(),
          solution);
        time.advance_time();

        constraint_matrix.distribute(solution);

        if (time.get_step_number() % 10 == 0)
          output_results(time.get_current_time(),
                         time.get_step_number(),
                         method);
      }
  }

//  @sect4{Diffusion::embedded_explicit_method}  这个函数是嵌入式显式方法的驱动。它需要更多的参数。

// - coarsen_param：当误差低于阈值时，乘以当前时间步长的系数。

// - refine_param: 当误差高于阈值时，乘以当前时间步长的系数。

// - min_delta: 可接受的最小时间步长。

// - max_delta: 可接受的最大时间步长。

// - refine_tol：时间步长超过的阈值。

// - coarsen_tol：阈值，低于该阈值的时间步长将被粗化。

// 嵌入方法使用一个猜测的时间步长。如果使用这个时间步长的误差太大，时间步长将被缩小。如果误差低于阈值，则在下一个时间步长时将尝试更大的时间步长。  delta_t_guess 是由嵌入式方法产生的猜测的时间步长。总之，时间步长有可能以三种方式修改。

// - 在 TimeStepping::EmbeddedExplicitRungeKutta::evolve_one_time_step(). 内减少或增加时间步长。

// - 使用计算出的  delta_t_guess  。

// - 自动调整最后一个时间步长，以确保模拟在  final_time  处精确结束。这种调整是在DiscreteTime实例中处理的。

  unsigned int Diffusion::embedded_explicit_method(
    const TimeStepping::runge_kutta_method method,
    const unsigned int                     n_time_steps,
    const double                           initial_time,
    const double                           final_time)
  {
    const double time_step =
      (final_time - initial_time) / static_cast<double>(n_time_steps);
    const double coarsen_param = 1.2;
    const double refine_param  = 0.8;
    const double min_delta     = 1e-8;
    const double max_delta     = 10 * time_step;
    const double refine_tol    = 1e-1;
    const double coarsen_tol   = 1e-5;

    solution = 0.;
    constraint_matrix.distribute(solution);

    TimeStepping::EmbeddedExplicitRungeKutta<Vector<double>>
      embedded_explicit_runge_kutta(method,
                                    coarsen_param,
                                    refine_param,
                                    min_delta,
                                    max_delta,
                                    refine_tol,
                                    coarsen_tol);
    output_results(initial_time, 0, method);
    DiscreteTime time(initial_time, final_time, time_step);
    while (time.is_at_end() == false)
      {
        const double new_time =
          embedded_explicit_runge_kutta.evolve_one_time_step(
            [this](const double time, const Vector<double> &y) {
              return this->evaluate_diffusion(time, y);
            },
            time.get_current_time(),
            time.get_next_step_size(),
            solution);
        time.set_next_step_size(new_time - time.get_current_time());
        time.advance_time();

        constraint_matrix.distribute(solution);

        if (time.get_step_number() % 10 == 0)
          output_results(time.get_current_time(),
                         time.get_step_number(),
                         method);

        time.set_desired_next_step_size(
          embedded_explicit_runge_kutta.get_status().delta_t_guess);
      }

    return time.get_step_number();
  }

//  @sect4{Diffusion::run}

// 下面是该程序的主要功能。在顶部，我们创建网格（一个[0,5]x[0,5]的正方形）并对其进行四次细化，得到一个有16乘16单元的网格，共256个。 然后我们将边界指示器设置为1，用于边界中 $x=0$ 和 $x=5$ 的部分。

  void Diffusion::run()
  {
    GridGenerator::hyper_cube(triangulation, 0., 5.);
    triangulation.refine_global(4);

    for (const auto &cell : triangulation.active_cell_iterators())
      for (const auto &face : cell->face_iterators())
        if (face->at_boundary())
          {
            if ((face->center()[0] == 0.) || (face->center()[0] == 5.))
              face->set_boundary_id(1);
            else
              face->set_boundary_id(0);
          }

// 接下来，我们设置线性系统并为其填充内容，以便在整个时间步进过程中使用它们。

    setup_system();

    assemble_system();

// 最后，我们使用命名空间TimeStepping中实现的几种Runge-Kutta方法来解决扩散问题，每次都会在结束时输出误差。(正如介绍中所解释的，由于精确解在最后时间为零，所以误差等于数值解，只需取解向量的 $l_2$ 准则即可计算出来。)

    unsigned int       n_steps      = 0;
    const unsigned int n_time_steps = 200;
    const double       initial_time = 0.;
    const double       final_time   = 10.;

    std::cout << "Explicit methods:" << std::endl;
    explicit_method(TimeStepping::FORWARD_EULER,
                    n_time_steps,
                    initial_time,
                    final_time);
    std::cout << "   Forward Euler:            error=" << solution.l2_norm()
              << std::endl;

    explicit_method(TimeStepping::RK_THIRD_ORDER,
                    n_time_steps,
                    initial_time,
                    final_time);
    std::cout << "   Third order Runge-Kutta:  error=" << solution.l2_norm()
              << std::endl;

    explicit_method(TimeStepping::RK_CLASSIC_FOURTH_ORDER,
                    n_time_steps,
                    initial_time,
                    final_time);
    std::cout << "   Fourth order Runge-Kutta: error=" << solution.l2_norm()
              << std::endl;
    std::cout << std::endl;

    std::cout << "Implicit methods:" << std::endl;
    implicit_method(TimeStepping::BACKWARD_EULER,
                    n_time_steps,
                    initial_time,
                    final_time);
    std::cout << "   Backward Euler:           error=" << solution.l2_norm()
              << std::endl;

    implicit_method(TimeStepping::IMPLICIT_MIDPOINT,
                    n_time_steps,
                    initial_time,
                    final_time);
    std::cout << "   Implicit Midpoint:        error=" << solution.l2_norm()
              << std::endl;

    implicit_method(TimeStepping::CRANK_NICOLSON,
                    n_time_steps,
                    initial_time,
                    final_time);
    std::cout << "   Crank-Nicolson:           error=" << solution.l2_norm()
              << std::endl;

    implicit_method(TimeStepping::SDIRK_TWO_STAGES,
                    n_time_steps,
                    initial_time,
                    final_time);
    std::cout << "   SDIRK:                    error=" << solution.l2_norm()
              << std::endl;
    std::cout << std::endl;

    std::cout << "Embedded explicit methods:" << std::endl;
    n_steps = embedded_explicit_method(TimeStepping::HEUN_EULER,
                                       n_time_steps,
                                       initial_time,
                                       final_time);
    std::cout << "   Heun-Euler:               error=" << solution.l2_norm()
              << std::endl;
    std::cout << "                   steps performed=" << n_steps << std::endl;

    n_steps = embedded_explicit_method(TimeStepping::BOGACKI_SHAMPINE,
                                       n_time_steps,
                                       initial_time,
                                       final_time);
    std::cout << "   Bogacki-Shampine:         error=" << solution.l2_norm()
              << std::endl;
    std::cout << "                   steps performed=" << n_steps << std::endl;

    n_steps = embedded_explicit_method(TimeStepping::DOPRI,
                                       n_time_steps,
                                       initial_time,
                                       final_time);
    std::cout << "   Dopri:                    error=" << solution.l2_norm()
              << std::endl;
    std::cout << "                   steps performed=" << n_steps << std::endl;

    n_steps = embedded_explicit_method(TimeStepping::FEHLBERG,
                                       n_time_steps,
                                       initial_time,
                                       final_time);
    std::cout << "   Fehlberg:                 error=" << solution.l2_norm()
              << std::endl;
    std::cout << "                   steps performed=" << n_steps << std::endl;

    n_steps = embedded_explicit_method(TimeStepping::CASH_KARP,
                                       n_time_steps,
                                       initial_time,
                                       final_time);
    std::cout << "   Cash-Karp:                error=" << solution.l2_norm()
              << std::endl;
    std::cout << "                   steps performed=" << n_steps << std::endl;
  }
} // namespace Step52

//  @sect3{The main() function}

// 下面的 main 函数与前面的例子类似，不需要注释。

int main()
{
  try
    {
      Step52::Diffusion diffusion;
      diffusion.run();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    };

  return 0;
}


#include <iostream>
#include "BenchTimer.h"
#include <Eigen/Dense>
#include <map>
#include <vector>
#include <string>
#include <sstream>
using namespace Eigen;

std::map<std::string,Array<float,1,8,DontAlign|RowMajor> > results;
std::vector<std::string> labels;
std::vector<Array2i> sizes;

template<typename Solver,typename MatrixType>
EIGEN_DONT_INLINE
void compute_norm_equation(Solver &solver, const MatrixType &A) {
  if(A.rows()!=A.cols())
    solver.compute(A.transpose()*A);
  else
    solver.compute(A);
}

template<typename Solver,typename MatrixType>
EIGEN_DONT_INLINE
void compute(Solver &solver, const MatrixType &A) {
  solver.compute(A);
}

template<typename Scalar,int Size>
void bench(int id, int rows, int size = Size)
{
  typedef Matrix<Scalar,Dynamic,Size> Mat;
  typedef Matrix<Scalar,Dynamic,Dynamic> MatDyn;
  typedef Matrix<Scalar,Size,Size> MatSquare;
  Mat A(rows,size);
  A.setRandom();
  if(rows==size)
    A = A*A.adjoint();
  BenchTimer t_llt, t_ldlt, t_lu, t_fplu, t_qr, t_cpqr, t_cod, t_fpqr, t_jsvd, t_bdcsvd;

  int svd_opt = ComputeThinU|ComputeThinV;

  int tries = 5;
  int rep = 1000/size;
  if(rep==0) rep = 1;
//   rep = rep*rep;

  LLT<MatSquare> llt(size);
  LDLT<MatSquare> ldlt(size);
  PartialPivLU<MatSquare> lu(size);
  FullPivLU<MatSquare> fplu(size,size);
  HouseholderQR<Mat> qr(A.rows(),A.cols());
  ColPivHouseholderQR<Mat> cpqr(A.rows(),A.cols());
  CompleteOrthogonalDecomposition<Mat> cod(A.rows(),A.cols());
  FullPivHouseholderQR<Mat> fpqr(A.rows(),A.cols());
  JacobiSVD<MatDyn> jsvd(A.rows(),A.cols());
  BDCSVD<MatDyn> bdcsvd(A.rows(),A.cols());

  BENCH(t_llt, tries, rep, compute_norm_equation(llt,A));
  BENCH(t_ldlt, tries, rep, compute_norm_equation(ldlt,A));
  BENCH(t_lu, tries, rep, compute_norm_equation(lu,A));
  if(size<=1000)
    BENCH(t_fplu, tries, rep, compute_norm_equation(fplu,A));
  BENCH(t_qr, tries, rep, compute(qr,A));
  BENCH(t_cpqr, tries, rep, compute(cpqr,A));
  BENCH(t_cod, tries, rep, compute(cod,A));
  if(size*rows<=10000000)
    BENCH(t_fpqr, tries, rep, compute(fpqr,A));
  if(size<500) // JacobiSVD is really too slow for too large matrices
    BENCH(t_jsvd, tries, rep, jsvd.compute(A,svd_opt));
//   if(size*rows<=20000000)
    BENCH(t_bdcsvd, tries, rep, bdcsvd.compute(A,svd_opt));

  results["LLT"][id] = t_llt.best();
  results["LDLT"][id] = t_ldlt.best();
  results["PartialPivLU"][id] = t_lu.best();
  results["FullPivLU"][id] = t_fplu.best();
  results["HouseholderQR"][id] = t_qr.best();
  results["ColPivHouseholderQR"][id] = t_cpqr.best();
  results["CompleteOrthogonalDecomposition"][id] = t_cod.best();
  results["FullPivHouseholderQR"][id] = t_fpqr.best();
  results["JacobiSVD"][id] = t_jsvd.best();
  results["BDCSVD"][id] = t_bdcsvd.best();
}

int main()
{
  labels.push_back("LLT");
  labels.push_back("LDLT");
  labels.push_back("PartialPivLU");
  labels.push_back("FullPivLU");
  labels.push_back("HouseholderQR");
  labels.push_back("ColPivHouseholderQR");
  labels.push_back("CompleteOrthogonalDecomposition");
  labels.push_back("FullPivHouseholderQR");
  labels.push_back("JacobiSVD");
  labels.push_back("BDCSVD");

  for(int i=0; i<labels.size(); ++i)
    results[labels[i]].fill(-1);

  const int small = 8;
  sizes.push_back(Array2i(small,small));
  sizes.push_back(Array2i(100,100));
  sizes.push_back(Array2i(1000,1000));
  sizes.push_back(Array2i(4000,4000));
  sizes.push_back(Array2i(10000,small));
  sizes.push_back(Array2i(10000,100));
  sizes.push_back(Array2i(10000,1000));
  sizes.push_back(Array2i(10000,4000));

  using namespace std;

  for(int k=0; k<sizes.size(); ++k)
  {
    cout << sizes[k](0) << "x" << sizes[k](1) << "...\n";
    bench<float,Dynamic>(k,sizes[k](0),sizes[k](1));
  }

  cout.width(32);
  cout << "solver/size";
  cout << "  ";
  for(int k=0; k<sizes.size(); ++k)
  {
    std::stringstream ss;
    ss << sizes[k](0) << "x" << sizes[k](1);
    cout.width(10); cout << ss.str(); cout << " ";
  }
  cout << endl;

  for(int i=0; i<labels.size(); ++i)
  {
    cout.width(32); cout << labels[i]; cout << "  ";
    ArrayXf r = (results[labels[i]]*100000.f).floor()/100.f;
    for(int k=0; k<sizes.size(); ++k)
    {
      cout.width(10);
      if(r(k)>=1e6)  cout << "-";
      else           cout << r(k);
      cout << " ";
    }
    cout << endl;
  }

  // HTML output
  cout << "
" << endl;
  cout << "
solver/size	" << endl;
  for(int k=0; k<sizes.size(); ++k)
    cout << "  " << sizes[k](0) << "x" << sizes[k](1) << "	";
  cout << "
" << endl;
  for(int i=0; i<labels.size(); ++i)
  {
    cout << "<tr";
    if(i%2==1) cout << " class=\"alt\"";
    cout << ">" << labels[i] << "	";
    ArrayXf r = (results[labels[i]]*100000.f).floor()/100.f;
    for(int k=0; k<sizes.size(); ++k)
    {
      if(r(k)>=1e6) cout << "-	";
      else
      {
        cout << "" << r(k);
        if(i>0)
          cout << " (x" << numext::round(10.f*results[labels[i]](k)/results["LLT"](k))/10.f << ")";
        if(i<4 && sizes[k](0)!=sizes[k](1))
          cout << " *";
        cout << "	";
      }
    }
    cout << "
" << endl;
  }
  cout << "
" << endl;

//   cout << "LLT                             (ms)  " << (results["LLT"]*1000.).format(fmt) << "\n";
//   cout << "LDLT                             (%)  " << (results["LDLT"]/results["LLT"]).format(fmt) << "\n";
//   cout << "PartialPivLU                     (%)  " << (results["PartialPivLU"]/results["LLT"]).format(fmt) << "\n";
//   cout << "FullPivLU                        (%)  " << (results["FullPivLU"]/results["LLT"]).format(fmt) << "\n";
//   cout << "HouseholderQR                    (%)  " << (results["HouseholderQR"]/results["LLT"]).format(fmt) << "\n";
//   cout << "ColPivHouseholderQR              (%)  " << (results["ColPivHouseholderQR"]/results["LLT"]).format(fmt) << "\n";
//   cout << "CompleteOrthogonalDecomposition  (%)  " << (results["CompleteOrthogonalDecomposition"]/results["LLT"]).format(fmt) << "\n";
//   cout << "FullPivHouseholderQR             (%)  " << (results["FullPivHouseholderQR"]/results["LLT"]).format(fmt) << "\n";
//   cout << "JacobiSVD                        (%)  " << (results["JacobiSVD"]/results["LLT"]).format(fmt) << "\n";
//   cout << "BDCSVD                           (%)  " << (results["BDCSVD"]/results["LLT"]).format(fmt) << "\n";
}


#include "KDE.hpp"
#include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm/for_each.hpp>
#include <boost/range/irange.hpp>
#include <boost/range/numeric.hpp>
#include <random>
#include <math.h>

using namespace std;
using namespace delphi::utils;
using boost::irange, boost::adaptors::transformed, boost::lambda::_1;

double sample_from_normal(
    std::mt19937 gen,
    double mu = 0.0, /**< The mean of the distribution.*/
    double sd = 1.0  /**< The standard deviation of the distribution.*/
) {
  normal_distribution<> d{mu, sd};
  return d(gen);
}

KDE::KDE(std::vector<double> v) : dataset(v) {

  // Compute the bandwidth using Silverman's rule
  mu = mean(v);
  auto X = v | transformed(_1 - mu);

  // Compute standard deviation of the sample.
  size_t N = v.size();
  double stdev = sqrt(inner_product(X, X, 0.0) / (N - 1));
  bw = pow(4 * pow(stdev, 5) / (3 * N), 1 / 5);
}

KDE::KDE(vector<double> thetas, int n_bins)  : n_bins(n_bins) {
  this->dataset = thetas;
  this->mu = mean(thetas);
  double small_count = 0.00001; // To avoid log(0)
  this->log_prior_hist = vector<double>(n_bins, small_count);
  this->delta_theta = M_PI / n_bins;

  int highest_freq = 0;
  int highest_freq_bin = 0;

//  int bin_lo = n_bins - 1;
//  int bin_hi = 0;

  for (double theta : thetas) {
    theta = theta < 0 ? M_PI + theta : theta;

    int bin = this->theta_to_bin(theta);
//    bin_lo = bin < bin_lo ? bin : bin_lo;
//    bin_hi = bin > bin_hi ? bin : bin_hi;

    this->log_prior_hist[bin] += 1;

    if (highest_freq < this->log_prior_hist[bin]) {
      highest_freq = this->log_prior_hist[bin];
      highest_freq_bin = bin;
    }
  }

//  if (bin_lo != bin_hi && bin_lo != (bin_hi + 1) % n_bins)

  this->most_probable_theta = highest_freq_bin * this->delta_theta +
                              this->delta_theta / 2;
  double n_points = thetas.size() + small_count * n_bins;

  for (double & count : this->log_prior_hist) {
    count /= n_points;
    count = log(count);
  }
}

void KDE::set_num_bins(int n_bins) {
  this->n_bins = n_bins;
  this->delta_theta = M_PI / n_bins;
}

int KDE::theta_to_bin(double theta) {
    return floor(theta / this->delta_theta);
}

vector<double> KDE::resample(int n_samples,
                             std::mt19937& gen,
                             uniform_real_distribution<double>& uni_dist,
                             normal_distribution<double>& norm_dist) {
  vector<double> samples(n_samples);

  for (int i : irange(0, n_samples)) {
    double element = select_random_element(dataset, gen, uni_dist);

    // Transform the sampled values using a Gaussian distribution
    // ~ ( sampled value, bw)
    // We sample from a standard Gaussian and transform that sample
    // to the desired Gaussian distribution by
    // μ + σ * standard Gaussian sample
    samples[i] = element + bw * norm_dist(gen);
  }

  return samples;
}

// This Should not be called!
double KDE::pdf(double x) {
  double p = 0.0;
  size_t N = this->dataset.size();
  for (double elem : this->dataset) {
    double x1 = exp(-sqr(x - elem) / (2 * sqr(bw)));
    x1 /= N * bw * sqrt(2 * M_PI);
    p += x1;
  }
  return p;
}

vector<double> KDE::pdf(vector<double> v) {
  vector<double> values;
  for (double elem : v) {
    values.push_back(pdf(elem));
  }
  return values;
}

double KDE::logpdf(double x) { return log(pdf(x)); }


#include <stdio.h>
#include <math.h>
#include <iostream>
#include <Eigen/Eigen>

Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> loge(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> in)
{
  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> out = in;

  out = out.array() + 1e-7f;

  for(int i=0; i<in.cols(); i++)
    {
      out(0, i) = logf(out(0,i));
    }

  return out;
}

struct NNLayer {
  struct NNLayer *next;
  struct NNLayer *back;

  NNLayer() : next(NULL), back(NULL)
    {
    };

  ~NNLayer() {};

  virtual Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> forward(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> m) = 0;

  virtual void back_propagation() = 0;

  void setNext(struct NNLayer *n)
    {
      next = n;
      n->back = this;
    };

  struct NNLayer * getNext()
    {
      return next;
    };

};

class Activation : public NNLayer {
  public:
    virtual Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> forward(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> m)
      {
      };
};

struct AffineLayer : public NNLayer {
  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> w;
  Eigen::Matrix<float, 1, Eigen::Dynamic> bias;
  Eigen::Matrix<float, 1, Eigen::Dynamic> output;
  bool needActivation;

  AffineLayer() : NNLayer(), needActivation(true)
    {
    }

  AffineLayer(int raws, int cols) : NNLayer(), needActivation(true)
    {
      resize(raws, cols);
    }

  ~AffineLayer() {};

  void resize(int raws, int cols)
    {
      w.resize(raws, cols);
      bias.resize(1, cols);
      output.resize(1, cols);

      w      = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>::Random(raws, cols);
      bias   = Eigen::Matrix<float, 1, Eigen::Dynamic>::Random(1, cols);
      output = Eigen::Matrix<float, 1, Eigen::Dynamic>::Random(1, cols);
    }

  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> forward(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> m)
    {
      output = (m * w) - bias;
      if (needActivation) activate();

      if (next != NULL)
        {
          return next->forward(output);
        }
      else
        {
          return output;
        }
    }

  void back_propagation()
    {
    };

  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> activate()
    {
      for(int i=0; i<output.cols(); i++)
        {
          output(0, i) = 1.0/(1.0 + exp(output(0, i)));
        }
      return output;
    }

  void setActivation(bool isneed)
    {
      needActivation = isneed;
    }
};

class NeuralNetwork {
  NNLayer *top_layer;
  NNLayer *last_layer;
  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> output;

  public:
    NeuralNetwork()
      {
        AffineLayer * tmplayers = new AffineLayer[3];
        tmplayers[0].resize(2,3);
        tmplayers[1].resize(3,5);
        tmplayers[2].resize(5,2);
        // tmplayers[2].setActivation(false);

        tmplayers[0].setNext(&tmplayers[1]);
        tmplayers[1].setNext(&tmplayers[2]);

        top_layer  = &tmplayers[0];
        last_layer = &tmplayers[2];
      };
    ~NeuralNetwork()
      {
        delete [] top_layer;
      };

    Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> forward(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> input)
      {
        output = top_layer->forward(input);
        return output;
      };

    void back_propagation()
      {
      };

    float cross_entropy(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> in)
      {
        Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> tmp;
        float out = 0.f;
        tmp = in.array() * loge(output).array();
        for(int i=0; i<tmp.cols(); i++)
          {
            out -= tmp(0, i);
          }
        return out;
      };
};

int main(void)
{
  NeuralNetwork nn;

  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> out;
  Eigen::Matrix<float, 1, 2> input;
  Eigen::Matrix<float, 1, 2> expect;
  input << 3, 3;

  expect << 2, 5;

  std::cout << "input = " << input << std::endl;
  std::cout << "expect = " << expect << std::endl;
  std::cout << "input * expect = " << input.array() * expect.array() << std::endl;

  std::cout << "input = " << input << std::endl;
  input = input.array() + 1;
  std::cout << "input = " << input << std::endl;

  out = nn.forward( input );

  std::cout << "out = " << out << std::endl;

  std::cout << "CrossEnt = " << nn.cross_entropy(expect) << std::endl;

  return 0;
}