Grover's algorithm

#include <iostream>
#include <vector>
#include <cmath>

#define pi 3.1415926535897932384626433832795028841971

using namespace std;

class Grover{
private:
    int n , target;

public:
    Grover(int _n , int _target){
        n = _n;
        target = _target;
    }

    pair<int , double> Solve(){ //{Результат , ймовірність}
        double t = 1.0 / sqrt(2);
        vector<vector<double>> X = {{0 , 1} , {1 , 0}};
        vector<vector<double>> H = {{t , t} , {t , -t}};
        vector<vector<double>> Hn = {{1}};

        int N = 1 << n;

        for(int i = 0 ; i < n ; ++i){
            Hn = Kron(Hn , H);
        }

        vector<double> bs0(N , 0);
        bs0[0] = 1;

        auto T = Mult(bs0 , bs0);
        vector<vector<double>> tmp = {{2}};
        auto S0 = Kron(tmp , T);

        for(int i = 0 ; i < N ; ++i){
            --S0[i][i];
        }

        auto Q = Query();

        tmp = {{1 , 0} , {0 , 1}};

        auto A = Mult(Mult(Hn , S0) , Hn);

        auto G = Mult(Kron(A , tmp) , Q);

        int pw = floor((pi / 4.0) * pow(2 , 1.0 * n / 2.0));

        auto GG = Mpow(G , pw);

        vector<vector<double>> l = {{1 , 0}};
        tmp.clear();
        tmp.push_back(bs0);

        auto P = Mult(Mult(GG , Kron(Hn , Mult(H , X))) , Kron(tmp , l));

        double p = 0;
        double m = 0;

        for(int i = 0 ; i < N ; ++i){
            double pr = P[2 * i][0] * P[2 * i][0] + P[2 * i + 1][0] * P[2 * i + 1][0];

            if(pr > p + 1e-7){
                m = i;
                p = pr;
            }
        }

        return {m , p};
    }
private:
    vector<vector<double>> Kron(vector<vector<double>> X , vector<vector<double>> Y){
        vector<vector<double>> res(X.size() * Y.size() , vector<double>(X[0].size() * Y[0].size()));

        for(int i = 0 ; i < X.size() ; ++i){
            for(int j = 0 ; j < X[0].size() ; ++j){
                auto V = Y;
                for(int x = 0 ; x < Y.size() ; ++x){
                    for(int y = 0 ; y < Y[0].size() ; ++y){
                        res[x + i * Y.size()][y + j * Y[0].size()] = X[i][j] * Y[x][y];
                    }
                }
            }
        }
        return res;
    }

    vector<vector<double>> Mult(vector<double> X , vector<double> Y){
        vector<vector<double>> res(X.size() , vector<double>(Y.size()));
        for(int i = 0 ; i < X.size() ; ++i){
            for(int j = 0 ; j < Y.size() ; ++j){
                res[i][j] = X[i] * Y[j];
            }
        }
        return res;
    }

    vector<vector<double>> Mult(vector<vector<double>> X , vector<vector<double>> Y){
        vector<vector<double>> res(X.size() , vector<double>(Y[0].size()));
        for(int i = 0 ; i < X.size() ; ++i){
            for(int j = 0 ; j < Y[0].size() ; ++j){
                for(int k = 0 ; k < Y.size() ; ++k){
                    res[i][j] += X[i][k] * Y[k][j];
                }
            }
        }
        return res;
    }

    vector<vector<double>> Query(){
        int N = 1 << n;
        vector<vector<double>> A(N , vector<double>(N , 0));
        for(int i = 0 ; i < N ; ++i){
            A[i][i] = 1.0;
        }
        vector<vector<double>> tmp = {{1 , 0} , {0 , 1}};
        A[target][target] = -1;

        return Kron(A , tmp);
    }

    vector<vector<double>> Mpow(vector<vector<double>> A, int p){
        vector<vector<double>> res(A.size() , vector<double>(A.size() , 0.0));
        for(int i = 0 ; i < A.size() ; ++i){
            res[i][i] = 1.0;
        }

        while(p){
            if(p & 1){
                res = Mult(res , A);
                --p;
            }else{
                A = Mult(A , A);
                p /= 2;
            }
        }
        return res;
    }

};

int main() {
//  ios_base::sync_with_stdio(false);
//  cin.tie(NULL);
    cout.setf(ios::fixed);
    cout.precision(10);
    Grover grover(8 , 255);//(n , target). , 0 <= target < 2^n
    auto ans = grover.Solve();
    cout << "Target is " << ans.first << " with probability " << ans.second << endl;

    return 0;
}