// Compile command: g++ -Wall -Wextra -std=c++17 -O2 -pthread -I/usr/include/eigen3 -I/usr/include/python3.8 -o triangular_cone_sigmax triangular_cone_sigmax.cpp -lpython3.8
#include <iostream>
#include <cmath>
#include <complex>
#include <Eigen/Dense>
#include <vector>
#include <utility>
#include <functional>
#include <thread>
#include <map>
#include <string>
#include <algorithm>
#include <ctime>
#include <iomanip>
#include <fstream>
#include <filesystem>
#include "matplotlibcpp.h"

namespace plt = matplotlibcpp;
using Eigen::MatrixXd;
using Eigen::Matrix2cd;
using Eigen::Vector2cd;
using std::vector;
using std::sqrt;
using std::cos;
using std::acos;
using std::sin;
using std::atan2;
using namespace std::complex_literals;

typedef std::complex<double> cd;
typedef vector<vector<vector<cd>>> grid_t;
typedef std::function<Matrix2cd(int, double, double)> gen_coin_t;

// Settings here
int num_steps = 10000;
int initialState = 0; // See main function for details
double eps = 0.01;
double dy = sqrt(3);
bool bugLambda = true; // Set to true to set Lambda = I_2
bool plotCone = false; // Set to true to plot the results in the cone coordinates
bool showVectField = true; // Set to true to show the vector field in original coordinates
int ymin = -50, ymax = 50;
int xmin = 2*ymin-2, xmax = 2*ymax+2;
int ntriangles_x = xmax-xmin+1;
int ntriangles_y = ymax-ymin+1;
int center[2] = {-xmin, -ymin};
std::string prefix;
bool sigmax_pattern[11];
vector<std::string> initialStateName = {"square", "shiftright", "shiftdl", "center", "bothsides", "almostcenter"};

std::time_t now = time(0);
std::tm *ltm = localtime(&now);

grid_t zerogrid() {
    return vector<vector<vector<cd>>>(ntriangles_x, vector<vector<cd>> (ntriangles_y, vector<cd> (3, 0. + 0i)));
}
grid_t nangrid() {
    return vector<vector<vector<cd>>>(ntriangles_x, vector<vector<cd>> (ntriangles_y, vector<cd> (3, std::nan("") + 0i)));
}

grid_t grid = zerogrid();

Matrix2cd H, Q;

double sumAmplitudes() {
    double ret = 0.0;
    for(int x = xmin; x <= xmax; x++)
        for(int y = ymin; y <= ymax; y++)
            for(int side = 0; side < 3; side++) {
                cd val = grid[x+center[0]][y+center[1]][side];
                ret += std::real(val*std::conj(val));
            }
    return ret;
}

void normalizeGrid() {
    double target = sumAmplitudes();
    double mul = 1/sqrt(target);
    if(target == 0)
        return;
    for(int x = xmin; x <= xmax; x++)
        for(int y = ymin; y <= ymax; y++)
            for(int side = 0; side < 3; side++)
                grid[x+center[0]][y+center[1]][side] *= mul;
}

inline int modulo(int a, int b) {
    return (a%b+b)%b;
}

void init_HQ() {
    H << 1, 1, 1, -1;
    H /= sqrt(2);
    Q << 1, -1i, 1, 1i;
    Q /= sqrt(2);
}

Matrix2cd gen_Ui(double thetai) {
    Matrix2cd ret;
    ret <<
        cos(thetai/2), sin(thetai/2),
        -sin(thetai/2), cos(thetai/2);
    return ret;
}

inline int sign(double d) {
    if(d>=0)
        return 1;
    else
        return -1;
}

inline double sq(double d) {
    return d*d;
}

inline double correct_fmod(const double a, const double b) {
    return std::fmod(std::fmod(a,b)+b, b);
}

inline double principal_measure(const double theta) {
    double ret = correct_fmod(theta, 2*M_PI);
    if(ret > M_PI)
        ret -= 2*M_PI;
    return ret;
}

Matrix2cd lamb(double rx, double ry) {
    Matrix2cd ret;
    if(bugLambda)
        ret = Matrix2cd::Identity();
    else if(rx == 0 && ry == 0) {
        ret <<
            6./5.*cos(-5.*M_PI/6.), -6./5.*sin(-5.*M_PI/6.),
            6./5.*sin(-5.*M_PI/6.), 5./5.*cos(-5.*M_PI/6.);
    }
    else {
        double r = 5./6.*sqrt(rx*rx+ry*ry);
        double thetap = atan2(ry, rx);
        double theta2 = principal_measure(thetap + 5*M_PI/6);
        double theta = 6./5.*theta2;
        double x = r*cos(theta), y = r*sin(theta);
        ret <<
            6./5.*(x/r*cos(thetap) + y/r*sin(thetap)),
            6./5.*(y/r*cos(thetap) - x/r*sin(thetap)),
            6./5.*(x/r*sin(thetap) - y/r*cos(thetap)),
            6./5.*(y/r*sin(thetap) + x/r*cos(thetap));
    }
    return ret;
}

vector<cd> l(double rx, double ry) {
    Matrix2cd lam = lamb(rx, ry);
    double sqrt3 = sqrt(3.0);
    return {lam(0,0), lam(1,0)/sqrt3, -lam(1,0)/sqrt3, lam(0,1), lam(1,1)/sqrt3, -lam(1,1)/sqrt3};
}

double gen_theta(int i, double rx, double ry) {
    return std::real(M_PI/2 + sqrt(eps)*l(rx, ry)[i]);
}

Matrix2cd gen_U(int i, double rx, double ry) {
    return gen_Ui(gen_theta(i, rx, ry));
}

Matrix2cd gen_Ubis(int i, double rx, double ry) {
    return gen_U(i+3, rx, ry);
}

Matrix2cd gen_Ustar(int i, double rx, double ry) {
    return gen_U(i, rx, ry).adjoint();
}

Matrix2cd gen_Ubisstar(int i, double rx, double ry) {
    return gen_Ustar(i+3, rx, ry);
}

grid_t shift(grid_t grid) {
    grid_t ngrid = zerogrid();
    for(int x = xmin; x <= xmax; x++) {
        for(int y = ymin; y <= ymax; y++) {
            for(int i = 0; i < 3; i++) {
                int iprec = ((i-1)%3+3)%3;
                ngrid[x+center[0]][y+center[1]][i] = grid[x+center[0]][y+center[1]][iprec];
            }
        }
    }
    return ngrid;
}

std::pair<double, double> real_coords(int iside, int x, int y, bool show = false) {
    double dec;
    if(show)
        dec = .4;
    else
        dec = .5;
    double xcoord, ycoord;
    if((x+y)%2==0) {
        if(iside == 0) {
            xcoord = x-dec;
            ycoord = (y+.5)*dy;
        }
        else if(iside == 1) {
            xcoord = x+dec;
            ycoord = (y+.5)*dy;
        }
        else {
            xcoord = x;
            ycoord = (y+.5+dec)*dy;
        }
    }
    else {
        if(iside == 0) {
            xcoord = x+dec;
            ycoord = (y+.5)*dy;
        }
        else if(iside == 1) {
            xcoord = x-dec;
            ycoord = (y+.5)*dy;
        }
        else {
            xcoord = x;
            ycoord = (y+.5-dec)*dy;
        }
    }
    return std::make_pair(sqrt(eps)*xcoord, sqrt(eps)*ycoord);
}

std::pair<double, double> cone_coords(int iside, int x, int y, bool show = false) {
    double rx, ry;
    std::tie(rx, ry) = real_coords(iside, x, y, show);
    double r = 5./6.*sqrt(rx*rx+ry*ry);
    double theta_before = atan2(ry, rx);
    double theta_rotate = principal_measure(theta_before+5*M_PI/6.);
    double theta = 6./5.*theta_rotate;
    //std::cerr << "theta " << theta_before*180/M_PI << " -> " << theta_rotate*180/M_PI << " -> " << theta*180/M_PI << std::endl;
    return std::make_pair(r*cos(theta), r*sin(theta));
}

const int DELTAS[][2] = {{-1,0}, {1,0}, {0,1}};
const int NUM_THREADS = 8;

void applyCoinsPartial(grid_t &ngrid, grid_t &grid, gen_coin_t &gen_coin, int loc_xmin, int loc_xmax, bool is_sigmax) {
    for(int x = loc_xmin; x < loc_xmax; x++) {
        for(int y = ymin; y <= ymax; y++) {
            if((x+y)%2 || (x>y && y>=0))
                continue;
            for(int iside = 0; iside < 3; iside++) {
                cd thisval = grid[x+center[0]][y+center[1]][iside];
                int xo = x + DELTAS[iside][0], yo = y + DELTAS[iside][1];
                //if(xo > xmax || xo < xmin || yo > ymax || yo < ymin || (xo > yo && yo >= 0)) { // pas de propagation aux bords
                if(xo > xmax || xo < xmin || yo > ymax || yo < ymin || (!is_sigmax && xo > yo && yo >= 0)) { // pas de propagation aux bords
                    ngrid[x+center[0]][y+center[1]][iside] = thisval;
                    continue;
                }
                else if(y == -1 && x >= 1 && iside == 2) {
                    int xo = x/2, yo = x/2;
                    ngrid[x+center[0]][y+center[1]][iside] = std::exp(1.0i * M_PI/3.) * grid[xo+center[0]][yo+center[1]][1];
                }
                else if(x == y && x >= 0 && iside == 1) {
                    int xo = 2*x+1, yo = -1;
                    ngrid[x+center[0]][y+center[1]][iside] = std::exp(-1.0i * M_PI/3.) * grid[xo+center[0]][yo+center[1]][2];
                }
                else {
                    cd otherval = grid[modulo(xo+center[0], ntriangles_x)][modulo(yo+center[1], ntriangles_y)][iside];
                    Vector2cd vect;
                    vect << thisval, otherval;
                    double rx, ry;
                    std::tie(rx, ry) = real_coords(iside, x, y);
                    Matrix2cd coin = gen_coin(iside, rx, ry);
                    Vector2cd newvect = coin*vect;
                    ngrid[x+center[0]][y+center[1]][iside] = newvect(0);
                    ngrid[modulo(xo+center[0], ntriangles_x)][modulo(yo+center[1], ntriangles_y)][iside] = newvect(1);
                }
            }
        }
    }
}

grid_t applyCoins(grid_t grid, gen_coin_t gen_coin, bool is_sigmax, bool multithread = true) {
    grid_t ngrid = nangrid();
    if(!multithread) {
        for(int x = xmin; x <= xmax; x++) {
            for(int y = ymin; y <= ymax; y++) {
                if((x+y)%2 || (x>y && y>=0)) // pentagone : on enlève un bout de 60°
                    continue;
                for(int iside = 0; iside < 3; iside++) {
                    cd thisval = grid[x+center[0]][y+center[1]][iside];
                    int xo = x + DELTAS[iside][0], yo = y + DELTAS[iside][1];
                    if(xo > xmax || xo < xmin || yo > ymax || yo < ymin || (!is_sigmax && xo > yo && yo >= 0)) { // pas de propagation aux bords
                        ngrid[x+center[0]][y+center[1]][iside] = thisval;
                        continue;
                    }
                    else if(y == -1 && x >= 1 && iside == 2) {
                        int xo = x/2, yo = x/2;
                        ngrid[x+center[0]][y+center[1]][iside] = std::exp(1.0i * M_PI/3.) * grid[xo+center[0]][yo+center[1]][1];
                    }
                    else if(x == y && x >= 0 && iside == 1) {
                        int xo = 2*x+1, yo = -1;
                        ngrid[x+center[0]][y+center[1]][iside] = std::exp(-1.0i * M_PI/3.) * grid[xo+center[0]][yo+center[1]][2];
                    }
                    else {
                        cd otherval = grid[modulo(xo+center[0], ntriangles_x)][modulo(yo+center[1], ntriangles_y)][iside];
                        Vector2cd vect;
                        vect << thisval, otherval;
                        double rx, ry;
                        std::tie(rx, ry) = real_coords(iside, x, y);
                        Matrix2cd coin = gen_coin(iside, rx, ry);
                        Vector2cd newvect = coin*vect;
                        ngrid[x+center[0]][y+center[1]][iside] = newvect(0);
                        ngrid[modulo(xo+center[0], ntriangles_x)][modulo(yo+center[1], ntriangles_y)][iside] = newvect(1);
                    }
                }
            }
        }
    }
    else {
        std::thread threads[NUM_THREADS];
        int delta_x = ntriangles_x/NUM_THREADS;
        for(int iThread = 0; iThread < NUM_THREADS-1; iThread++) {
            threads[iThread] = std::thread(applyCoinsPartial, std::ref(ngrid), std::ref(grid), std::ref(gen_coin), xmin+iThread*delta_x, xmin+(iThread+1)*delta_x, is_sigmax);
        }
        threads[NUM_THREADS-1] = std::thread(applyCoinsPartial, std::ref(ngrid), std::ref(grid), std::ref(gen_coin), xmin+(NUM_THREADS-1)*delta_x, xmax+1, is_sigmax);
        for(int iThread = 0; iThread < NUM_THREADS; iThread++)
            threads[iThread].join();
    }
    // We may miss some sides of 1 (tip up) triangles which are not adjacent to a valid 0 triangle
    for(int x = xmin; x <= xmax; x++)
        for(int y = ymin; y <= ymax; y++)
            for(int side = 0; side < 3; side++) {
                cd &newval = ngrid[x+center[0]][y+center[1]][side];
                if(std::isnan(std::real(newval)))
                    newval = grid[x+center[0]][y+center[1]][side];
            }
    return ngrid;
}

void plotVectorField(double minx, double maxx, double miny, double maxy, int gridstep = 20) {
    vector<double> xloc, yloc;
    vector<double> vectx, vecty;
    double dx = (maxx-minx)/gridstep, dy = (maxy-miny)/gridstep;
    for(double x = minx; x <= maxx; x += dx)
        for(double y = miny; y <= maxy; y += dy) {
            for(int i = 0; i < 2; i++) {
                xloc.push_back(x);
                yloc.push_back(y);
                Matrix2cd deform = lamb(x,y);
                vectx.push_back(std::real(deform(0,i)));
                vecty.push_back(std::real(deform(1,i)));
            }
        }
    plt::quiver(xloc, yloc, vectx, vecty, {{"pivot","tail"}, {"color", "grey"}});
}

void plot(int iGrid = -1) {
    std::cerr << "Plotting " << iGrid << std::endl;
    PyObject *fig;
    if(plotCone) {
        fig = plt::figure_size(1000,1000);
        plt::xlim(xmin*sqrt(eps), xmax*sqrt(eps));
        plt::ylim(ymin*sqrt(eps)*dy, ymax*sqrt(eps)*dy);
        plt::set_aspect_equal();
        vector<double> xlist, ylist, colorlist;
        vector<double> listvals;
        for(int x = xmin; x <= xmax; x++) {
            for(int y = ymin; y <= ymax; y++) {
                for(int iside = 0; iside < 3; iside++) {
                    cd val = grid[x+center[0]][y+center[1]][iside];
                    double col = std::real(val*std::conj(val));
                    listvals.push_back(col);
                }
            }
        }
        std::sort(listvals.rbegin(), listvals.rend());
        double maxi = (listvals[0]+listvals[1])/2;
        double threshold = maxi/10;
        for(int x = xmin; x <= xmax; x++) {
            for(int y = ymin; y <= ymax; y++) {
                for(int iside = 0; iside < 3; iside++) {
                    cd val = grid[x+center[0]][y+center[1]][iside];
                    double col = std::real(val*std::conj(val));
                    if(col > threshold) {
                        double rx, ry;
                        std::tie(rx, ry) = cone_coords(iside, x, y, true);
                        xlist.push_back(rx);
                        ylist.push_back(ry);
                        colorlist.push_back(col);
                    }
                }
            }
        }
        if(maxi == 0.0)
            maxi = 1.0;
        maxi *= 0.6;
        plt::scatter_colored(xlist, ylist, colorlist, 5, {{"cmap","gist_heat_r"}, {"vmin", "0"}, {"vmax", std::to_string(maxi)}});
    }
    else {
        fig = plt::figure_size(1000,1000);
        plt::set_aspect_equal();
        vector<vector<double>> imgrid(ymax-ymin+1, vector<double>(xmax-xmin+1, 0.0));
        vector<double> listvals;
        for(int y = ymin; y <= ymax; y++) {
            for(int x = xmin; x <= xmax; x++) {
                double sum = 0.0;
                for(int iside = 0; iside < 3; iside++) {
                    cd val = grid[x+center[0]][y+center[1]][iside];
                    double col = std::real(val*std::conj(val));
                    sum += col;
                }
                listvals.push_back(sum);
                imgrid[y+center[1]][x+center[0]] = sum;
            }
        }
        std::sort(listvals.rbegin(), listvals.rend());
        double maxi = (listvals[0]+listvals[1])/2;
        if(maxi == 0.0)
            maxi = 1.0;
        maxi *= .6;

        double minx = sqrt(eps)*(xmin-.5);
        double maxx = sqrt(eps)*(xmax+.5);
        double miny = sqrt(eps)*dy*(ymin-.5);
        double maxy = sqrt(eps)*dy*(ymax+.5);
        plt::imshow(imgrid, {minx, maxx, miny, maxy}, {{"origin", "lower"}, {"cmap", "gist_heat_r"}, {"vmin", "0.0"}, {"vmax", std::to_string(maxi)}});
        plt::plot({0.0, maxy/tan(M_PI/3)}, {0.0, maxy}, {{"color","red"}});
        plt::plot({0.0, xmax*sqrt(eps)}, {0.0, 0.0}, {{"color","red"}});
        if(showVectField)
            plotVectorField(xmin*sqrt(eps), xmax*sqrt(eps), ymin*sqrt(eps)*dy, ymax*sqrt(eps)*dy, 20);
    }
    std::ostringstream filename;
    filename << prefix << "_" << iGrid << ".png";
    plt::save(prefix + "/" + filename.str());
    plt::clf();
    plt::close(fig);
    Py_DECREF(fig);

    // Plot around dislocation line
    vector<double> xlist2, ylist2;
    for(int x = 0; x <= xmax; x++) {
        vector<int> klist;
        if((x+1)%2)
            klist = {1, 2, 0};
        else
            klist = {0, 2, 1};
        for(int k : klist) {
            double rx, ry;
            std::tie(rx, ry) = real_coords(k, x, -1);
            xlist2.push_back(rx);
            cd val = grid[x+center[0]][center[1]-1][k];
            double col = std::real(val*std::conj(val));
            ylist2.push_back(col);
        }
    }
    plt::plot(xlist2, ylist2);
    std::ostringstream filename_disloc;
    filename_disloc << prefix << "_dislocation_" << iGrid << ".png";
    plt::save(prefix + "/" + filename_disloc.str());
    plt::cla();
    plt::clf();
    plt::close();
    std::cerr << "Done plotting " << iGrid << std::endl;
}

void step_walk(int step = -1) {
    std::cerr << "Begin step " << step << std::endl;
    grid = applyCoins(grid, [](int i, double rx, double ry) {
        Matrix2cd ret = H;
        return ret;
    }, sigmax_pattern[0]);
    for(int j = 0; j < 3; j++) {
        grid = applyCoins(grid, gen_Ustar, sigmax_pattern[1]);
        grid = shift(grid);
        grid = applyCoins(grid, [](int i, double rx, double ry) {
            Matrix2cd ret = gen_U(((i-1)%3+3)%3, rx, ry);
            return ret;
        }, sigmax_pattern[2]);
    }
    for(int j = 0; j < 3; j++) {
        grid = applyCoins(grid, gen_U, sigmax_pattern[3]);
        grid = shift(grid);
        grid = applyCoins(grid, [](int i, double rx, double ry) {
            Matrix2cd ret = gen_Ustar(((i-1)%3+3)%3, rx, ry);
            return ret;
        }, sigmax_pattern[4]);
    }
    grid = applyCoins(grid, [](int i, double rx, double ry) {
        Matrix2cd ret = Q*H;
        return ret;
    }, sigmax_pattern[5]);
    for(int j = 0; j < 3; j++) {
        grid = applyCoins(grid, gen_Ubisstar, sigmax_pattern[6]);
        grid = shift(grid);
        grid = applyCoins(grid, [](int i, double rx, double ry) {
            Matrix2cd ret = gen_Ubis(((i-1)%3+3)%3, rx, ry);
            return ret;
        }, sigmax_pattern[7]);
    }
    for(int j = 0; j < 3; j++) {
        grid = applyCoins(grid, gen_Ubis, sigmax_pattern[8]);
        grid = shift(grid);
        grid = applyCoins(grid, [](int i, double rx, double ry) {
            Matrix2cd ret = gen_Ubisstar(((i-1)%3+3)%3, rx, ry);
            return ret;
        }, sigmax_pattern[9]);
    }
    grid = applyCoins(grid, [](int i, double rx, double ry) {
        Matrix2cd ret = Q.adjoint();
        return ret;
    }, sigmax_pattern[10]);
    std::cerr << "Total amplitude: " << sumAmplitudes() << "\n";
    //normalizeGrid();
    //std::cerr << "After normalization: " << sumAmplitudes() << "\n";
    std::cerr << "End step " << step << std::endl;
}

void print_params() {
    std::ofstream ostream(prefix + "/settings.txt");
    ostream << "num_steps = " << num_steps << "\n";
    ostream << "eps = " << eps << "\n";
    ostream << "xmin = " << xmin << "\n";
    ostream << "xmax = " << xmax << "\n";
    ostream << "ymin = " << ymin << "\n";
    ostream << "ymax = " << ymax << "\n";
    ostream << "sigma_x pattern: ";
    for(int i = 0; i < 11; i++)
        ostream << (int)sigmax_pattern[i];
    ostream.close();
}

void listInitialStates() {
    for(size_t i = 0; i < initialStateName.size(); i++)
        std::cerr << "- " << i << ": " << initialStateName[i] << "\n";
}

int main(int argc, char **argv)
{
    if(argc <= 3) {
        std::cerr << "Usage: " << std::string(argv[0]) << " <initial state> <plot cone> <sigmax pattern>\n";
        std::cerr << "Initial states:\n";
        listInitialStates();
        std::cerr << "<plot cone> should be 1 if the figure should be plotted in cone coordinates, 0 otherwise.\n";
        std::cerr << "<sigmax pattern> should be a sequence of 11 ones and zeroes, a 1 at position i meaning that sigma_x should be applied on the dislocation line at coin i and 0 meaning that the identity should be applied instead.\n";
        return 1;
    }
    initialState = std::atoi(argv[1]);
    if(initialState < 0 || initialState >= (int)initialStateName.size()) {
        std::cerr << "Invalid initial state " << initialState << ". List of possible states:\n";
        listInitialStates();
        return 2;
    }
    plotCone = (bool) std::atoi(argv[2]);
    if(std::strlen(argv[3]) != 11) {
        std::cerr << "Invalid sigma_x pattern: mismatching length, should be 11 characters\n";
        return 3;
    }
    for(int i = 0; i < 11; i++) {
        if(argv[3][i] == '0')
            sigmax_pattern[i] = false;
        else if(argv[3][i] == '1')
            sigmax_pattern[i] = true;
        else {
            std::cerr << "Invalid sigma_x pattern: expected 0 or 1, got " << argv[3][i] << "\n";
            return 3;
        }
    }
    std::ostringstream str;
    str <<
        "simul_conesigmax_" << initialStateName[initialState];
    if(plotCone)
        str << "_conecoord_";
    else
        str << "_altcoord_";
    str <<
        std::setw(4) << std::setfill('0') << ltm->tm_year+1900 << "-" <<
        std::setw(2) << ltm->tm_mon+1 << "-" <<
        std::setw(2) << ltm->tm_mday << "_" <<
        std::setw(2) << ltm->tm_hour << "-" <<
        std::setw(2) << ltm->tm_min << "-" <<
        std::setw(2) << ltm->tm_sec;
    prefix = str.str();
    std::filesystem::create_directory(prefix);
    init_HQ();
    print_params();
    // Initial state
    if(initialState == 0) {
        for(int x = xmin/5; x <= xmax/5; x++)
            for(int y = ymin/5; y <= ymax/5; y++)
                for(int k = 0; k < 3; k++)
                    if(x <= y || y < 0)
                        grid[center[0]+x][center[1]+y][k] = 1;
        normalizeGrid();
    }
    // Shifted
    else if(initialState == 1) {
        for(int k = 0; k < 3; k++)
            grid[center[0]+xmax/2-1][center[1]-1][k]=1/sqrt(3);
    }
    // Shifted (in another way)
    else if(initialState == 2) {
        for(int k = 0; k < 3; k++)
            grid[center[0]+xmin/4+1][center[1]+ymin/4+1][k]=1/sqrt(3);
    }
    // Centered
    else if(initialState == 3) {
        vector<vector<int>> centercoord = {{-1,0},{0,0},{-1,-1},{0,-1},{1,-1}};
        for(vector<int> &coord : centercoord)
            for(int k = 0; k < 3; k++)
                grid[center[0]+coord[0]][center[1]+coord[1]][k]=1/sqrt(3*centercoord.size());
    }
    // Shifted, both sides of the line
    else if(initialState == 4) {
        for(int k=0; k<3; k++) {
            grid[center[0]+10][center[1]+10][k] = 1/sqrt(6);
            grid[center[0]+21][center[1]-1][k] = 1/sqrt(6);
        }
    }
    // Almost centered
    else if(initialState == 5) {
        for(int k = 0; k < 3; k++)
            grid[center[0]-1][center[1]-1][k] = 1/sqrt(3);
    }
    plot(0);
    for(int i = 0; i < num_steps; i++) {
        step_walk(i);
        if(((i+1)%10) == 0)
            plot(i+1);
    }
}


#include <stdio.h>
#include <iostream>

#include <Eigen/Geometry>

#include "../libfovis/refine_motion_estimate.hpp"

#define dump(v) std::cerr << #v << " : " << (v) << "\n"
#define dumpT(v) std::cerr << #v << " : " << (v).transpose() << "\n"

using namespace fovis;

static inline Eigen::Isometry3d
isometryFromXYZRollPitchYaw(const Eigen::Matrix<double, 6, 1>& params)
{
  Eigen::Isometry3d result;

  double roll = params(3), pitch = params(4), yaw = params(5);
  double halfroll = roll / 2;
  double halfpitch = pitch / 2;
  double halfyaw = yaw / 2;
  double sin_r2 = sin(halfroll);
  double sin_p2 = sin(halfpitch);
  double sin_y2 = sin(halfyaw);
  double cos_r2 = cos(halfroll);
  double cos_p2 = cos(halfpitch);
  double cos_y2 = cos(halfyaw);

  Eigen::Quaterniond quat(
    cos_r2 * cos_p2 * cos_y2 + sin_r2 * sin_p2 * sin_y2,
    sin_r2 * cos_p2 * cos_y2 - cos_r2 * sin_p2 * sin_y2,
    cos_r2 * sin_p2 * cos_y2 + sin_r2 * cos_p2 * sin_y2,
    cos_r2 * cos_p2 * sin_y2 - sin_r2 * sin_p2 * cos_y2);

  result.setIdentity();
  result.translate(params.head<3>());
  result.rotate(quat);

  return result;
}

static inline Eigen::Vector3d
isometryGetRollPitchYaw(const Eigen::Isometry3d& M)
{
  Eigen::Quaterniond q(M.rotation());
  double roll_a = 2 * (q.w()*q.x() + q.y()*q.z());
  double roll_b = 1 - 2 * (q.x()*q.x() + q.y()*q.y());
  double pitch_sin = 2 * (q.w()*q.y() - q.z()*q.x());
  double yaw_a = 2 * (q.w()*q.z() + q.x()*q.y());
  double yaw_b = 1 - 2 * (q.y()*q.y() + q.z()*q.z());

  return Eigen::Vector3d(atan2(roll_a, roll_b),
      asin(pitch_sin),
      atan2(yaw_a, yaw_b));
}

int main(int argc, char** argv)
{

  int num_points = 9;

  Eigen::Matrix<double, 4, Eigen::Dynamic> points_xyz(4, num_points);
  Eigen::Matrix<double, 4, Eigen::Dynamic> transformed_xyz(4, num_points);
  Eigen::Matrix<double, 2, Eigen::Dynamic> ref_projections(2, num_points);

  double tx    = 1.0;
  double ty    = 1.5;
  double tz    = 0.5;
  double roll  = 10 * (M_PI / 180);
  double pitch =  1 * (M_PI / 180);
  double yaw   =  5 * (M_PI / 180);
  //	tx = 0;
  //	ty = 0;
  //	tz = 0;
  //	roll = 0;
  //	pitch = 0;
  //	yaw = 1 * (M_PI / 180);

  Eigen::Matrix<double, 6, 1> params;
  params << tx, ty, tz, roll, pitch, yaw;
  Eigen::Isometry3d true_motion = isometryFromXYZRollPitchYaw(params);
  double fx = 528;
  double cx = 320;
  double cy = 240;

  Eigen::Matrix<double, Eigen::Dynamic, 2> tmp(num_points, 2);
  tmp <<
    0, 0,
    320, 0,
    640, 0,
    0, 240,
    320, 240,
    640, 240,
    0, 480,
    320, 480,
    640, 480;
  ref_projections = tmp.transpose();
  double depths[] = {
    1, 0.5, 0.75,
    100, 1, 0.75,
    0.75, 0.5, 1,
  };

  Eigen::Matrix<double, 3, 4> K;
  K << fx, 0, cx, 0,
    0, fx, cy, 0,
    0, 0, 1, 0;

  for(int i=0; i<num_points; i++) {
    points_xyz(0, i) = depths[i] * (ref_projections(0, i) - cx) / fx;
    points_xyz(1, i) = depths[i] * (ref_projections(1, i) - cy) / fx;
    points_xyz(2, i) = depths[i];
    points_xyz(3, i) = 1;

    Eigen::Vector3d uvw = K * points_xyz.col(i);
    uvw(0) /= uvw(2);
    uvw(1) /= uvw(2);
    uvw(2) = 1;

    transformed_xyz.col(i) = true_motion.inverse().matrix() * points_xyz.col(i);

    Eigen::Vector4d t = transformed_xyz.col(i);
    Eigen::Vector4d p = points_xyz.col(i);

    printf("%3d : %6.2f %6.2f  ->  %6.2f %6.2f %6.2f  -> %7.3f %7.3f %7.3f\n",
        i,
        uvw(0), uvw(1),
        p(0), p(1), p(2),
        t(0), t(1), t(2));
  }
  printf("=======\n");

  Eigen::Isometry3d initial_estimate;
  initial_estimate.setIdentity();
  Eigen::Vector3d initial_rpy = isometryGetRollPitchYaw(initial_estimate);
  Eigen::Vector3d initial_trans = initial_estimate.translation();

  Eigen::Isometry3d estimate = refineMotionEstimate(transformed_xyz,
      ref_projections,
      fx, cx, cy,
      initial_estimate, 6);

  Eigen::Matrix<double, 3, 4> P = K * estimate.matrix();

  Eigen::Vector3d estimated_rpy = isometryGetRollPitchYaw(estimate);
  Eigen::Vector3d estimated_trans = estimate.translation();
  printf("       Estimate   True    Initial\n"
      "tx:    %6.2f %6.2f %6.2f\n"
      "ty:    %6.2f %6.2f %6.2f\n"
      "tz:    %6.2f %6.2f %6.2f\n"
      "roll:  %6.2f %6.2f %6.2f\n"
      "pitch: %6.2f %6.2f %6.2f\n"
      "yaw:   %6.2f %6.2f %6.2f\n",
      estimated_trans(0), tx, initial_trans(0),
      estimated_trans(1), ty, initial_trans(1),
      estimated_trans(2), tz, initial_trans(2),
      estimated_rpy(0) * 180 / M_PI, roll * 180 / M_PI,  initial_rpy(0) * 180 / M_PI,
      estimated_rpy(1) * 180 / M_PI, pitch * 180 / M_PI, initial_rpy(1) * 180 / M_PI,
      estimated_rpy(2) * 180 / M_PI, yaw * 180 / M_PI,   initial_rpy(2) * 180 / M_PI);

  // compute reprojection error
  for(int i=0; i<num_points; i++) {
    Eigen::Vector3d uvw = P * transformed_xyz.col(i);
    double u = uvw(0) / uvw(2);
    double v = uvw(1) / uvw(2);
    double ref_u = ref_projections(0, i);
    double ref_v = ref_projections(1, i);
    double err_u = u - ref_u;
    double err_v = v - ref_v;

    printf("%3d  %6.1f %6.1f -> %6.1f %6.1f (%6.2f %6.2f)\n", i, ref_u, ref_v, u, v, err_u, err_v);
  }

  return 0;
}


/* @copyright The code is licensed under the MIT License
 *            <https://opensource.org/licenses/MIT>,
 *            Copyright (c) 2020 Christian Eskil Vaugelade Berg
 * @author Christian Eskil Vaugelade Berg
*/
#pragma once

#include <orient/detail/so3_generator.hpp>

#include <Eigen/Dense>

namespace orient::detail {

template <typename Derived, typename Scalar = typename Eigen::DenseBase<Derived>::Scalar>
Eigen::Matrix<Scalar,3,3> skewSymmetric(Eigen::DenseBase<Derived> const& w) {
  return (Eigen::Matrix<Scalar,3,3>() << 0.0, -w(2), w(1), w(2), 0.0, -w(0), -w(1), w(0), 0.0).finished();
}

template <typename Derived, typename Scalar = typename Eigen::DenseBase<Derived>::Scalar>
std::pair<Eigen::Matrix<Scalar,3,3>, Eigen::Matrix<Scalar, 9, 3>> skewSymmetricWPD(Eigen::MatrixBase<Derived> const& w) {
  Eigen::Matrix<Scalar, 9, 3> J{};
  Eigen::Map<Eigen::Matrix<Scalar,3,3>>(J.template block<9,1>(0,0).data(), 3,3) = generator<Axis::x>;
  Eigen::Map<Eigen::Matrix<Scalar,3,3>>(J.template block<9,1>(0,1).data(), 3,3) = generator<Axis::y>;
  Eigen::Map<Eigen::Matrix<Scalar,3,3>>(J.template block<9,1>(0,2).data(), 3,3) = generator<Axis::z>;
  return std::make_pair(skewSymmetric(w), J);
}

template <typename Derived, typename Scalar = typename Eigen::DenseBase<Derived>::Scalar>
Eigen::Matrix<Scalar,3,1> unskewSymmetric(Eigen::MatrixBase<Derived> const& M)
{
  return (Eigen::Matrix<Scalar,3,1>() << - M(1,2), M(0,2), - M(0,1)).finished();
}

template <typename Derived, typename Scalar = typename Eigen::DenseBase<Derived>::Scalar>
std::pair<Eigen::Matrix<Scalar,3,1>, Eigen::Matrix<Scalar, 3, 9>> unskewSymmetricWPD(Eigen::MatrixBase<Derived> const& M)
{
  Eigen::Matrix<Scalar, 3, 9> J = Eigen::Matrix<Scalar, 3, 9>::Zero();
  J(0, 2*3 + 1) = -1.;
  J(1, 2*3) = 1.;
  J(2, 1*3) = -1.;
  return std::make_pair(unskewSymmetric(M), J);
}

}


#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


#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);

    }
  }

}


#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]));
	}
}


/*
 * 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 */


/*
 *
 * 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


// 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();
  }
}


#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