#Imports non Qiskit
import numpy as np
from numpy import pi
#Imports Qiskit
    #Généraux
from qiskit import QuantumCircuit, QuantumRegister, ClassicalRegister, execute, Aer, IBMQ
    #Aqua
from qiskit.aqua.algorithms import HHL, NumPyLSsolver
from qiskit.aqua.components.eigs import EigsQPE
from qiskit.aqua.components.reciprocals import LookupRotation
from qiskit.aqua.operators import MatrixOperator
from qiskit.aqua.components.initial_states import Custom
    #Plot tools
from qiskit.visualization import plot_histogram, plot_bloch_multivector
from qiskit_textbook.tools import array_to_latex
    #Autres
from qiskit.circuit import Gate
from qiskit.circuit.library import QFT
from qiskit.quantum_info import state_fidelity
from qiskit.providers.ibmq import least_busy         #QFT
from qiskit_textbook.tools import random_state
from qiskit.tools.monitor import job_monitor         #QFTe


from typing import Optional, List
import numpy as np

from qiskit import Aer, execute, QuantumRegister, QuantumCircuit, ClassicalRegister
                  
from qiskit.circuit.library import HGate, RYGate, ZGate, XGate, CSwapGate, SwapGate
from qiskit.quantum_info import Operator

from qiskit.aqua.components.initial_states import InitialState
from qiskit.aqua.utils.validation import validate_min
from qiskit.aqua.components.initial_states import Custom

from qiskit.visualization import plot_histogram

%config InlineBackend.figure_format = 'svg' # Makes the images look nice

'''
import sys
sys.setrecursionlimit(1000)
'''

# memory structure B_x proposed by (Prakash, 2014) to store the information of the classical vector b.
# structured slightly modified for complex vectors

class BinaryTree:
    def __init__(self, value, level=0,branch=0):
        self.value = value
        self.child_left = None
        self.child_right = None
        self.level = level
        self.branch = branch

    def insert_left(self, value, branch):
        if self.child_left == None:
            self.child_left = BinaryTree(value, self.get_level()+1, branch)
        else:
            new_node = BinaryTree(value, self.get_level()+1, branch)
            self.child_left.set_level(self.child_left.get_level()+1)
            self.child_left.set_branch(self.child_left.get_branch()*2)
            new_node.child_left = self.child_left
            self.child_left = new_node

    def insert_right(self, value, branch):
        if self.child_right == None:
            self.child_right = BinaryTree(value, self.get_level()+1, branch)
        else:
            new_node = BinaryTree(value, self.get_level()+1, branch)
            self.child_right.set_level(self.child_right.get_level()+1)
            self.child_right.set_branch(self.child_right.get_branch()*2+1)
            new_node.child_right = self.child_right
            self.child_right = new_node

    def get_value(self):
        return self.value

    def get_left(self):
        return self.child_left

    def get_right(self):
        return self.child_right
    
    def get_level(self):
        return self.level
    
    def get_branch(self):
        return self.branch
    
    def set_level(self, level):
        self.level = level
    
    def set_branch(self, branch):
        self.branch = branch
    
def show_tree(T):
    if T != None:
        return ([T.get_value(),T.get_level(),T.get_branch()],show_tree(T.get_left()),show_tree(T.get_right()))
    
def create_tree_from_vector(x: list):
    norm = np.linalg.norm(x)
    u_x = [x_i/norm for x_i in x]
    
    if(len(u_x)<1):
        print("Please enter a non-null vector")
    
    elif(len(u_x)==1):
        s = 1
        phase = np.angle(u_x[0])
        tree = BinaryTree([s, phase])
    
    else:
        tree = BinaryTree(1)
        u_x_left=u_x[0:(len(x)+1)//2]
        u_x_right=u_x[(len(x)+1)//2:len(x)]
        create_tree_recursively(u_x_left, tree, "left", tree.get_branch())
        create_tree_recursively(u_x_right, tree, "right", tree.get_branch())
    
    return norm, tree


def create_tree_recursively(x: list, tree: BinaryTree, side: str, branch: int):
    
    # adding the new node to the tree
    s = 0
    for i in range(len(x)):
        s += x[i].real**2+x[i].imag**2
    
    if(len(x) == 1):
        phase = np.angle(x[0])
        s=[s,phase]
    
    if(side == "left"):
        tree.insert_left(s, branch*2)
    elif(side == "right"):
        tree.insert_right(s, branch*2+1)
    else:
        print("problem in a switch case, create tree recursively") 
    
    # calling recursively the fonction
    if(len(x) > 1):
        x_left=x[0:(len(x)+1)//2]
        x_right=x[(len(x)+1)//2:len(x)]
        
        # creating a perfect binary tree (all interior nodes have two children and all leaves have the same depth or same level)
        if(len(x_left) == 1 and len(x_right) > 1):
            x_left = [x_left[0], 0]
        elif(len(x_right) == 1 and len(x_left) > 1):
            x_right = [x_right[0], 0]
            
        if(side == "left"):
            create_tree_recursively(x_left, tree.get_left(), "left", tree.get_left().get_branch())
            create_tree_recursively(x_right, tree.get_left(), "right", tree.get_left().get_branch())
        elif(side == "right"):
            create_tree_recursively(x_left, tree.get_right(), "left", tree.get_right().get_branch())
            create_tree_recursively(x_right, tree.get_right(), "right", tree.get_right().get_branch())
        else:
            print("Error: a branch of the tree has not been considered")
    else:
        return

# state preparation procedure of (Grover & Rudolph, 2002) to get |psi>=|b> from b
# procedure slightly modified for complex vectors

def load_vector_from_qRAM(x: list, 
                          qc: Optional[QuantumCircuit]=None, 
                          pos_init: int=0, 
                          normIntegerPrecision: int=0, 
                          normDecimalPrecision: int=0,
                          Qiskit_reading_direction: bool=False):
    
    norm, B_x = create_tree_from_vector(x)
    
    n = int(np.ceil(np.log(len(x))))
    if(n == 0):
        n = 1
    
    # Code if there is not a quantum circuit as an input
    if(qc is None):
        cr3 = ClassicalRegister(n)
        qr3 = QuantumRegister(n, 'tree')
        if(normIntegerPrecision > 0):
            cr1 = ClassicalRegister(normIntegerPrecision)
            qr1 = QuantumRegister(normIntegerPrecision, 'n_int')
            if(normDecimalPrecision > 0):
                cr2 = ClassicalRegister(normDecimalPrecision)
                qr2 = QuantumRegister(normDecimalPrecision, 'n_dec')
                qc = QuantumCircuit(qr1,qr2,qr3,cr1,cr2,cr3)
            else:
                qc = QuantumCircuit(qr1,qr3,cr1,cr3)
        else:
            if(normDecimalPrecision > 0):
                cr2 = ClassicalRegister(normDecimalPrecision)
                qr2 = QuantumRegister(normDecimalPrecision, 'n_dec')
                qc = QuantumCircuit(qr2,qr3,cr2,cr3)
            else:
                qc = QuantumCircuit(qr3,cr3)

                
    ## loading norm_l2(x)
    if(normIntegerPrecision + normDecimalPrecision > 0):
        norm_binary = getBinary(norm)
    
        # integer part of the norm
        if(len(norm_binary[0]) > normIntegerPrecision):
            for i in range(normIntegerPrecision):
                qc.x(pos_init+i)
        else:
            for i in range(len(norm_binary[0])):
                if(norm_binary[0][i] == '1'):
                    qc.x(pos_init+normIntegerPrecision-len(norm_binary[0])+i)
    
        # decimal part of the norm
        if(len(norm_binary[1]) > normDecimalPrecision):
            for i in range(normDecimalPrecision):
                if(norm_binary[1][i] == '1'):
                    qc.x(pos_init+normIntegerPrecision+i)
        else:
            for i in range(len(norm_binary[1])):
                if(norm_binary[1][i] == '1'):
                    qc.x(pos_init+normIntegerPrecision+i)
    
    ## loading u=x/norm_l2(x)
    if(n > 0):
        ## rotation level 1
        # getting the left and right child nodes
        B_l = B_x.get_left()
        B_r = B_x.get_right()
     
        # calculating the angle of rotation 'theta': cos('theta') = sqrt(value(u_l)/value(u))
        B_lIsALeave = False
        if(B_l.get_left() is None):
            B_lIsALeave = True
    
        if(B_lIsALeave):
            node_value = B_l.get_value()[0]
        else:
            node_value = B_l.get_value()
        
        theta_0 = 2*np.arccos(np.sqrt(node_value))
        qc.ry(theta_0, pos_init+normIntegerPrecision+normDecimalPrecision)    
    
    
        # processPhase if the nodes u_l and u_r are leaves (personal approach)
        isAComplexPair = False
        if(B_lIsALeave == True):
            if(B_l.get_value()[1] == 0.0 and B_r.get_value()[1] == 0.0):
                pass
            elif(B_l.get_value()[1] == 0.0 and B_r.get_value()[1] == np.pi):
                theta_0 = -theta_0
            elif(B_l.get_value()[1] == np.pi and B_r.get_value()[1] == 0.0):
                theta_0 = np.pi-theta_0
            elif(B_l.get_value()[1] == np.pi and B_r.get_value()[1] == np.pi):
                theta_0 = np.pi+theta_0
            else:
                isAComplexPair = True
    
        # changing the local phase if necessary using the quantum gate diag(e^{i*angle_l},e^{i*angle_r}) = X*U_1(angle_l)*X*U_1(angle_r)
        if(isAComplexPair == True):
            qc.u1(B_r.get_value()[1]-B_l.get_value()[1],0)
    
    if(n > 1):
        processNode(qc, pos_init+normIntegerPrecision+normDecimalPrecision, n, B_l)
        processNode(qc, pos_init+normIntegerPrecision+normDecimalPrecision, n, B_r)
    
    # swap the qubits in the quantum register if demanded (Qiskit_reading_direction == False). 
    if(Qiskit_reading_direction == False):
        for qubit in range(pos_init, pos_init+normIntegerPrecision//2):
            qc.swap(qubit, normIntegerPrecision-qubit-1)
        for qubit in range(pos_init+normIntegerPrecision, (pos_init+normIntegerPrecision+normDecimalPrecision)//2):
            qc.swap(qubit, normDecimalPrecision-qubit-1)
        for qubit in range(pos_init+normIntegerPrecision+normDecimalPrecision, (pos_init+normIntegerPrecision+normDecimalPrecision+n)//2):
            qc.swap(qubit, n-qubit-1)
    
    qc.barrier()
    
    return qc
    
def processNode(q_circuit: QuantumCircuit, first_qubit: int, sizeRegisterTree: int, u_x: BinaryTree):
    
    ## rotation level k > 1
    # getting the left and right child nodes
    u_l = u_x.get_left()
    u_r = u_x.get_right()
    
    # if value(u) = 0 then value(u_c) = 0 for all the child nodes in the subtree whose parent is u. Therefore, no calculation is performed.
    if(u_x.get_value() != 0):
        
        # calculating the angle of rotation 'theta': cos('theta') = sqrt(value(u_l)/value(u))
        u_lIsALeave = False
        if(u_l.get_left() is None):
            u_lIsALeave = True
        
        if(u_l is not None):
            if(u_lIsALeave):
                node_value = u_l.get_value()[0]
            else:
                node_value = u_l.get_value()
        
            theta = 2*np.arccos(np.sqrt(node_value/u_x.get_value()))
        else:
            print("Error: processNode() to a node without childs")
        
    
        # processPhase if the nodes u_l and u_r are leaves (personal approach)
        isAComplexPair = False
        if(u_lIsALeave):
            if(u_l.get_value()[1] == 0.0 and u_r.get_value()[1] == 0.0):
                pass
            elif(u_l.get_value()[1] == 0.0 and u_r.get_value()[1] == np.pi):
                theta = -theta
            elif(u_l.get_value()[1] == np.pi and u_r.get_value()[1] == 0.0):
                theta = np.pi-theta
            elif(u_l.get_value()[1] == np.pi and u_r.get_value()[1] == np.pi):
                theta = np.pi+theta
            else:
                isAComplexPair = True
    
        # choosing the quantum eigenstate associated to the parent node (the control eigenstate q_1q_2...q_{k-1})
        list_binary = getBinary(u_x.get_branch(), u_l.get_level()-1)[0]
        for i in range(len(list_binary)):
            if(list_binary[i] == '0'):
                #q_circuit.x(first_qubit+len(list_binary)-(i+1))
                q_circuit.x(first_qubit+i)
    
        ## Multi-controlled Rotation gate around the y-axis (*)
        qc_custom = QuantumCircuit(1)
        qc_custom.ry(theta,0)
        
        # changing the local phase if necessary using the quantum gate diag(e^{i*angle_l},e^{i*angle_r}) = X*U_1(angle_l)*X*U_1(angle_r)
        if(isAComplexPair == True):
            qc_custom.u1(u_r.get_value()[1]-u_l.get_value()[1],0)
        
        #MC_Ry = RYGate(theta).control(u_l.get_level()-1)
        MC_Ry = qc_custom.to_gate(label="Ry(\u03B8)*U1(\u03B2 - \u03B1)").control(u_l.get_level()-1)
        q_circuit.append(MC_Ry, [first_qubit+x_i for x_i in range(u_l.get_level())])
    
        # uncomputing the quantum eigenstate associated to the parent node
        list_binary = getBinary(u_x.get_branch(), u_l.get_level()-1)[0]
        for i in range(len(list_binary)):
            if(list_binary[i] == '0'):
                #q_circuit.x(first_qubit++len(list_binary)-(i+1))
                q_circuit.x(first_qubit+i)
    
        ## recursive call
        if(u_lIsALeave == False):
            q_circuit.barrier()
            processNode(q_circuit, first_qubit, sizeRegisterTree, u_l)
            processNode(q_circuit, first_qubit, sizeRegisterTree, u_r)
        else:
            return

    
def getBinary(number: float, integerPrecision: int=1):
    # integer part
    int_part = int(number)
    binary_int = format(int_part, '#0'+str(int(2+integerPrecision))+'b')[2:]
    
    # decimal part
    dec_part = number - float(int_part)
    binary_dec = ''
    while(dec_part > 0):
        dec_part = 2*dec_part
        if(dec_part >= 1):
            binary_dec += '1'
            dec_part = dec_part-1
        else:
            binary_dec += '0'
    return binary_int, binary_dec 

def hamiltonian_simulation(H,t,r = 1):
    """Prends un observable H (matrice numpy) et un temps t en entrée et renvoie opérateur d'évolution exp(-itH)"""
    
    
    def taylor(H,t,n=100):
        """Première implémentation naïve avec le developpement de taylor de l'exponentielle"""
        U = np.eye(len(H))
        for k in range(n):
            U = np.dot(U,(-1j)*t*H)/(n-k)
            U += np.eye(len(H))
        return U
    
    def trotter_suzuki(H,t,r):
        """Ici H doit être de la forme [A,B] où A[i] est la liste des termes non nuls dans la colonne i et B[i][A[i]] 
        leur valeurs"""
        def separate(H, L = []):
            S1 = [[i for i in range(len(H[0]))],[[0 for i in range(len(H[0]))] for j in range(len(H[0]))]]
            if L == []:
                for i in range(len(H[0])):
                    for j in range(len(H[0][i])):
                        if H[0][i][j] == i:
                            S1[0][i] = i
                            S1[1][i][i] = H[1][i][i]
                            del H[0][i][j]
                            break
                L += [S1]
                return separate(H,L)
                
            Hvide = True
            for k in range(len(H[0])):
                if H[0][k] != []:
                    Hvide = False
                    
            if Hvide:
                return L
            
            Colored = len(H[0])*[0]
            for i in range(len(H[0])):
                for j in range(len(H[0][i])):
                    if not(Colored[H[0][i][j]]):
                        Colored[H[0][i][j]] = 1
                        Colored[i] = 1
                        
                        S1[0][i] = H[0][i][j]
                        S1[0][H[0][i][j]] = i
                        
                        S1[1][i][H[0][i][j]] = H[1][i][H[0][i][j]]
                        S1[1][H[0][i][j]][i] = H[1][H[0][i][j]][i]
                        
                        temp = 0
                        for k in range(len(H[0][H[0][i][j]])):
                            if H[0][H[0][i][j]][k]==i:
                                temp = k
                        
                        del H[0][H[0][i][j]][temp]
                        del H[0][i][j]
                        break
            L += [S1]
            return separate(H,L)
            
            
            
        def fastexp(M,t):
            done = len(M[0])*[0]
            for k in range(len(done)):
                if not(done[k]):
                    if M[0][k] == k:
                        M[1][k][k] = np.exp(1j*t*M[1][k][k])
                        done[k] = 1
                    else:
                        if M[1][k][M[0][k]].imag == 0:
                            M[1][k][k] = np.cos(t*M[1][k][M[0][k]])
                            M[1][M[0][k]][M[0][k]] = np.cos(t*M[1][k][M[0][k]])
                            M[1][M[0][k]][k] = 1j*np.sin(t*M[1][k][M[0][k]])
                            M[1][k][M[0][k]] = 1j*np.sin(t*M[1][k][M[0][k]])
                        else:
                            M[1][k][k] = np.cos(t*M[1][k][M[0][k]])
                            M[1][M[0][k]][M[0][k]] = np.cos(t*M[1][k][M[0][k]])
                            M[1][M[0][k]][k] = np.sin(t*M[1][k][M[0][k]])
                            M[1][k][M[0][k]] = -np.sin(t*M[1][k][M[0][k]])
                        
                        done[k] = 1
                        done[M[0][k]] = 1
            return M
        
        
        L1 = separate(H)
        L = []
        for k in range(len(L1)):
            Sr = [[i for i in range(len(H[0]))],[[0 for i in range(len(H[0]))] for j in range(len(H[0]))]]
            Sc = [[i for i in range(len(H[0]))],[[0 for i in range(len(H[0]))] for j in range(len(H[0]))]]
            for i in range(len(L1[k][0])):
                Sr[0][i] = L1[k][0][i]
                Sc[0][i] = L1[k][0][i]               
                
                Sr[1][i][Sr[0][i]] = L1[k][1][i][L1[k][0][i]].real
                Sc[1][i][Sc[0][i]] = L1[k][1][i][L1[k][0][i]].imag
                
                
            L += [Sr]+[Sc]
                
        for k in range(len(L)):
            L[k] = fastexp(L[k],t/r)
        
        
        L1 = []
        for k in range(len(L)):
            L1 += [L[k][1]]
        
        return L1,r
        
        
    
    return trotter_suzuki(H,t,r)



def qft_rotations(circuit,position_initial,nombre_qubits):
    """Performs qft on the first n qubits ("nombre_qubits") in circuit (without swaps)"""
    if nombre_qubits == 0:
        return circuit
    nombre_qubits -= 1
    circuit.h(position_initial+nombre_qubits)
    for qubit in range(nombre_qubits):
        circuit.cu1(pi/2**(nombre_qubits-qubit), position_initial+qubit, position_initial+nombre_qubits)
    # At the end of our function, we call the same function again on
    # the next qubits (we reduced "nombre_qubits" by one earlier in the function)
    qft_rotations(circuit,position_initial,nombre_qubits)
    
def swap_registers(circuit,position_initial,nombre_qubits):
    for qubit in range(nombre_qubits//2):
        circuit.swap(position_initial+qubit,position_initial+nombre_qubits-qubit-1)
    return circuit

def qft(circuit: QuantumCircuit,position_initial: int,nombre_qubits: int):
    """QFT on the first n qubits ("nombre_qubits") in circuit"""
    qft_rotations(circuit,position_initial,nombre_qubits)
    swap_registers(circuit,position_initial,nombre_qubits)
    return circuit

# Let's see how it looks for a circuit with 4 qubits (intial state |0000>):
qc = QuantumCircuit(6)
circ_=qc
position_initial=1
n_qubits=4
qft(circ_,position_initial,n_qubits)
#qc.measure_all()
qc.draw('mpl')

def inverse_qft(circuit: QuantumCircuit, position_initial: int=0, nombre_qubits: int=2, nombre_total_qubits: int=2):
    """Does the inverse QFT on the first n qubits ("nombre_qubits") in circuit"""
    # First we create a QFT circuit of the correct size:
    qft_circ = qft(QuantumCircuit(nombre_total_qubits), position_initial, nombre_qubits)
    # Then we take the inverse of this circuit
    invqft_circ = qft_circ.inverse()
    # And add it to the first n qubits (nombre_qubits) in our existing circuit
    circuit.append(invqft_circ, circuit.qubits)
    return circuit.decompose() # .decompose() allows us to see the individual gates


nqubits = 4
n_total_qubits = 6
position_initial=1
number = 5
qc = QuantumCircuit(n_total_qubits)

'''
for qubit in range(nqubits):
    qc.h(qubit)
    

qc.u1(number*pi/4,0)
qc.u1(number*pi/2,1)
qc.u1(number*pi,2)
'''

qc.draw('mpl')

qc = inverse_qft(qc,position_initial,nqubits,n_total_qubits)
#qc.measure_all()
qc.draw('mpl')



def qft_dagger(circ_,position_initial,n_qubits):
    """n-qubit QFTdagger the first n qubits in circ"""
    for qubit in range(int(n_qubits/2)):
        circ_.swap(position_initial+qubit,position_initial+n_qubits-1-qubit)
    for j in range(0,n_qubits):
        for m in range(j):
            circ_.cp(-np.pi/float(2**(j-m)), position_initial+m, position_initial+j)
        circ_.h(position_initial+j)



qc = QuantumCircuit(6)
qc.draw('mpl')

circ_=qc
position_initial=1
n_qubits=4
qft_dagger(circ_,position_initial,n_qubits)
#qc.measure_all()
qc.draw('mpl')

# a: clock register (HHL+) or phase register (rapport intermédiaire) or ancillae register (Qiskit textbook chapter 3.6)
# q: input register (HHL+, rapport intermédiaire)
# s: ancilla register (HHL+) or auxiliary register (rapport intermédiaire, Qiskit textbook Chapter 3.6)

# global variables to test the second call to the Hamiltonian Simulation module 
list_CU_operators = None 
exponent_r = None
flag_get_CU_gate = False # variable to modify list_CU_operators each repetition of HHL


def get_CU_gate(num_qubits_a: int=1,
                    num_qubits_q: int=1,
                    t: int=50,
                    unitary_matrix: Optional[List]=None,               # parameter to test the validity of the model
                    matrix: Optional[List]=None,
                    nonzero_entries: Optional[List]=None,
                    inverse_gate: bool=False):           

    
    global list_CU_operators, exponent_r, flag_get_CU_gate
    
    # if qpe_hhl has called this function
    if(list_CU_operators is None or flag_get_CU_gate == False):
    
        ## get the matrices U_1,...,U_n and and the scalar r so that U=(U1*...*Un)*...*(U1*...*Un): r times "(U1*...*Un)".
        if(matrix is not None and nonzero_entries is not None):
            list_U_matrices, exponent_r = hamiltonian_simulation([nonzero_entries, matrix],t)
            dim_U_matrix = len(matrix)
        elif(unitary_matrix is not None):
            list_U_matrices = [unitary_matrix]                 
            exponent_r = 1
            dim_U_matrix = len(unitary_matrix)
        else:
            print("list_U_operators cannot be properly implemented. Provide a matrix or a valid operator (or see the Hamiltonien Simulation module).")
    
    
        ## construct the operators controlled-(Uj) to be applied in reverse order of qiskit convention
        list_CU_operators = []
        for k in range(len(list_U_matrices)):
            matrix_CU = np.identity(dim_U_matrix*2, dtype = 'complex_')
            for i in range(dim_U_matrix):
                for j in range(dim_U_matrix):
                    matrix_CU[dim_U_matrix+i][dim_U_matrix+j] = list_U_matrices[k][i][j]
            operator_CU = Operator(matrix_CU)
            list_CU_operators.append(operator_CU)
        
        flag_get_CU_gate = True
    else:
        flag_get_CU_gate = False
    
    ## convert the unitary operators into a quantum gates controlled-(U1*...*Un) to be applied r times
    qc_temp = QuantumCircuit(1+num_qubits_q) 
    prefix_gate_name = "C-U"
    
    for i in range(len(list_CU_operators)):
        suffix_gate_name  = str(i)
        if(inverse_gate):
            suffix_gate_name += "^dagger"
        qc_temp.append(list_CU_operators[i].to_instruction().copy(prefix_gate_name+suffix_gate_name), 
                       [x for x in range(num_qubits_q,-1,-1)])   # the order of qubits is reversed taking into account the Qiskit convention of ordering qubits  
       
    ## deciding if returning the CU_gate or its inverse
    CU_gate = qc_temp.to_gate()
    if(inverse_gate == False):
        return CU_gate, exponent_r 
    else:
        CU_gate_inv = CU_gate.inverse()
        CU_gate_inv.name = "C-U^dagger"
        return CU_gate_inv, exponent_r 
    
    
def qpe_hhl(circuit, 
            pos_init_qreg_a: int, 
            num_qubits_a: int, 
            pos_init_qreg_q: int, 
            num_qubits_q: int=1,
            unitary_matrix: Optional[List]=None,             # parameter to test the validity of the model
            matrix: Optional[List]=None,
            nonzero_entries: Optional[List]=None,
            t: int=45,
            measurement: bool=False,
            Qiskit_reading_direction: bool=True):              
    
    
    ## apply the Hadamard gate
    for qubit in range(pos_init_qreg_a, pos_init_qreg_a+num_qubits_a):
        circuit.h(qubit)
    
    ## create the controlled-U operator and turn it into a gate
    
    if (unitary_matrix != None) : 
        CU_gate, exponent_r = get_CU_gate(num_qubits_a, num_qubits_q, t, unitary_matrix, matrix, nonzero_entries)
    else : 
        CU_gate, exponent_r = get_CU_gate(num_qubits_a, num_qubits_q, t,None, matrix, nonzero_entries)
        
    ## apply the controlled-U^{2^j} gates
    repetitions = 1
    for qubit_qreg_a in range(pos_init_qreg_a, pos_init_qreg_a+num_qubits_a):
        for i in range(repetitions):
        
        # apply the Controlled-U gate (or the approximation: C-U=C-((U1*...*Un)^r) to the main circuit
            qubits_control = [qubit_qreg_a]
            qubits_controlled = [x for x in range(pos_init_qreg_q, pos_init_qreg_q+num_qubits_q)]
            
            for j in range(exponent_r):
                circuit.append(CU_gate.copy("controlled-U"), qubits_control+qubits_controlled)    
        repetitions *= 2
    
    
    ## apply inverse QFT
    qft_dagger(circuit, pos_init_qreg_a, num_qubits_a)
    

    # swap the qubits in the quantum register "a" if demanded (Qiskit_reading_direction == False). 
    if(Qiskit_reading_direction == False):
        for qubit_qreg_a in range(pos_init_qreg_a, pos_init_qreg_a+num_qubits_a//2):
            circuit.swap(qubit_qreg_a, num_qubits_a-qubit_qreg_a-1)
    
    # add a measurement section at the end of the circuit if necessary (measurement == True). 
    if measurement:
        c_a = ClassicalRegister(num_qubits_a, name='ca')
        circuit.add_register(c_a)
        circuit.barrier()    # the safeguard-barrier
        for qubit_qreg_a in range(pos_init_qreg_a, pos_init_qreg_a+num_qubits_a):
            circuit.measure(qubit_qreg_a, c_a[qubit_qreg_a-pos_init_qreg_a])
            
    

def qpedagger_hhl(circuit, 
            pos_init_qreg_a: int, 
            num_qubits_a: int, 
            pos_init_qreg_q: int, 
            num_qubits_q: int=1,
            unitar_matrix: Optional[List]=None,
            matrix: Optional[List]=None,
            nonzero_entries: Optional[List]=None,
            t: int=45,
            measurement: bool=False,
            Qiskit_reading_direction: bool=True):            # parameter to test the validity of the model
    
    # swap the qubits in the quantum register "a" if demanded (Qiskit_reading_direction == False). 
    if(Qiskit_reading_direction == False):
        for qubit_qreg_a in range(pos_init_qreg_a, pos_init_qreg_a+num_qubits_a//2):
            circuit.swap(qubit_qreg_a, num_qubits_a-qubit_qreg_a-1)
    
    ## apply QFT
    qft(circuit, pos_init_qreg_a, num_qubits_a)
    
    
    ## create the controlled-U^dagger operator and turn it into a gate
    if (unitar_matrix != None) : 
        CU_gate, exponent_r = get_CU_gate(num_qubits_a, num_qubits_q, t, unitary_matrix, matrix, nonzero_entries, inverse_gate=True)
    else : 
        CU_gate, exponent_r = get_CU_gate(num_qubits_a, num_qubits_q, t,None, matrix, nonzero_entries, inverse_gate=True)
        
    ## apply the controlled-U^{2^j} gates
    repetitions = 2**(num_qubits_a-1)
    for qubit_qreg_a in range(pos_init_qreg_a+num_qubits_a-1, pos_init_qreg_a-1, -1):
        for i in range(repetitions):
        
        # apply the Controlled-U gate (or the approximation: C-U=C-((U1*...*Un)^r) to the main circuit
            qubits_control = [qubit_qreg_a]
            qubits_controlled = [x for x in range(pos_init_qreg_q, pos_init_qreg_q+num_qubits_q)]
            
            for j in range(exponent_r):
                circuit.append(CU_gate.copy("controlled-U^dagger"), qubits_control+qubits_controlled)    

        repetitions //= 2
        
    
    
    ## apply the Hadamard gate
    for qubit in range(pos_init_qreg_a, num_qubits_a):
        circuit.h(qubit)

    
    # add a measurement section at the end of the circuit if necessary (measurement == True). 
    if measurement:
        c_a = ClassicalRegister(num_qubits_a, name='ca')
        circuit.add_register(c_a)
        circuit.barrier()    # the safeguard-barrier
        for qubit_qreg_a in range(pos_init_qreg_a, pos_init_qreg_a+num_qubits_a):
            circuit.measure(qubit_qreg_a, c_a[qubit_qreg_a-pos_init_qreg_a])
            
    

num_ancillae = 2          #n_l
num_state = 2             #n_b
t = 6*np.pi/8
qr_a = QuantumRegister(num_ancillae, name='theta')
qr_q = QuantumRegister(num_state, name='b')
c = ClassicalRegister(num_ancillae+num_state, name='cr')


# matrix A
#matrix = [[1, -1/3],
#          [-1/3, 1]]
matrix = [[1, -1/3,0,0],
          [-1/3, 1,0,0],
          [0,0,1, -1/3],
          [0,0,-1/3, 1]]

#nonzero_entries = [[0, 1],
 #                 [0, 1]]

nonzero_entries = [[0, 1, 2, 3],
                   [0, 1, 2, 3],
                   [0, 1, 2, 3],
                   [0, 1, 2, 3]]

initial_state = [1, 1, 1, 1]
#initial_state = [1, 1, 1, 1]

qc = QuantumCircuit(qr_q,qr_a,c)


# set the initial state using qiskit
state_in = Custom(num_qubits=num_state, state_vector = np.array(initial_state))
qc.data += state_in.construct_circuit('circuit', qr_q).data   #qc.data : Return the circuit data (instructions and context).


# call the modules hamiltonian simulation (classical) + QPE + QFT
qpe_hhl(circuit=qc, pos_init_qreg_a=2, num_qubits_a=num_ancillae, pos_init_qreg_q=0, num_qubits_q=2,
        matrix=matrix, nonzero_entries=nonzero_entries, t=t, measurement=False, Qiskit_reading_direction=True)
#qpedagger_hhl(circuit=qc, pos_init_qreg_a=0, num_qubits_a=num_ancillae, pos_init_qreg_q=num_ancillae, unitar_matrix=None, matrix=matrix, nonzero_entries=nonzero_entries, t=t, measurement=False, Qiskit_reading_direction=True)
qc.data += state_in.construct_circuit('circuit', qr_q).inverse().data
qc.draw(scale=0.8) #use "qc.decompose().draw(scale=0.8)" to see all the black-boxes (decomposed in the set of universal gates of Open-QASM: {u1, u2, u3, CNOT})

for i in range(num_ancillae+num_state):
    qc.measure(i,i)

backend = Aer.get_backend('qasm_simulator')
shots = 2048
results = execute(qc, backend=backend, shots=shots).result()
answer = results.get_counts()

plot_histogram(answer)

###########
####QFT####
###########

def qca_qft(qc,r):
    #Adds QFT operations on circuit qc, on the register indexed by r
    #cf. Qiskit textbook, 3.5, for explanation of the algorithm
    n=len(r)
    for i in range(n):
        qc.h(r[n-1-i])
        for j in range(n-i-1):
            qc.cp(pi/2**(n-1-i-j),r[j],r[n-1-i])
    for i in range(n//2):
        qc.swap(r[i],r[n-1-i])
        
def qca_qftdagger(qc,r):
    #Adds QFTdagger operations on circuit qc, on the register indexed by r
    n=len(r)
    for i in range(n//2):
        qc.swap(r[i],r[n-1-i])
    for i in reversed(range(n)):
        for j in reversed(range(n-i-1)):
            qc.cp(-pi/2**(n-1-i-j),r[j],r[n-1-i])
        qc.h(r[n-1-i])
        
def qca_c_qft(qc,r,pos_ctrl):
    #Does the same thing as qca_qft, but with all the gates controlled by qubit in position qctrl
    n=len(r)
    for i in range(n):
        qc.ch(pos_ctrl,r[n-1-i])
        for j in range(n-i-1):
            qc.mcp(pi/2**(n-1-i-j),[pos_ctrl,r[j]],r[n-1-i])
    for i in range(n//2):
        qc.cswap(pos_ctrl,r[i],r[n-1-i])
        
def qca_c_qftdagger(qc,r,pos_ctrl):
    #Does the same thing as qca_qftdagger, but with all the gates controlled by qubit in position qctrl
    n=len(r)
    for i in range(n//2):
        qc.cswap(pos_ctrl,r[i],r[n-1-i])
    for i in reversed(range(n)):
        for j in reversed(range(n-i-1)):
            qc.mcp(-pi/2**(n-1-i-j),[pos_ctrl,r[j]],r[n-1-i])
        qc.ch(pos_ctrl,r[n-1-i])
        
################
##Arithmétique##
################


#Addition
#########

def qca_adder(qc,a,b):
    #Input:
        #n+1 qubits of register indexed by a : (binary represantion in the computational basis of) a and |0\rangle on the last qubit
        #n+1 qubits of register indexed by b : |b\rangle on the first n qubits, |0\rangle on the last qubit (last qubit used to store last carry)
    #Output:
        #n qubits of register indexed by a : |a\rangle
        #n+1 qubits of register indexed by b : |a+b\rangle
    n=len(a)-1
    qca_qft(qc,b)
    qc.barrier()
    for i in range(n+1,0,-1):
        for j in range(i,0,-1):
            if j<=n:
                qc.cp(pi/2**(i-j),a[j-1],b[n+1-i])
    qc.barrier()
    qca_qftdagger(qc,b)

def qca_ccp(qc,theta,ctrl1,ctrl2,target):
    #Double controlled phase gate
    qc.cp(theta/2,ctrl2,target)
    qc.cx(ctrl1,ctrl2)
    qc.cp(-theta/2,ctrl2,target)
    qc.cx(ctrl1,ctrl2)
    qc.cp(theta/2,ctrl1,target)
    
def qca_c_adder(qc,a,b,pos_ctrl):
    #ctrl version of qca_adder
    n=len(a)-1
    qca_c_qft(qc,b,pos_ctrl)
    qc.barrier()
    for i in range(n+1,0,-1):
        for j in range(i,0,-1):
            if j<=n:
                qca_ccp(qc,pi/2**(i-j),pos_ctrl,a[j-1],b[n+1-i])
    qc.barrier()
    qca_c_qftdagger(qc,b,pos_ctrl)
    
#Pseudo-soustraction
####################

def qca_subber(qc,a,b):
    #Input:
        #n+1 qubits of register indexed by a : (binary represantion in the computational basis of) a and |0\rangle on the last qubit
        #n+1 qubits of register indexed by b : |b\rangle on the first n qubits, |0\rangle on the last qubit (last qubit used to store last carry)
    #Output:
        #n qubits of register indexed by a : |a\rangle
        #n+1 qubits of register indexed by b : |a-b\rangle
    n=len(a)-1
    qca_qft(qc,b)
    qc.barrier()
    for i in range(n+1,0,-1):
        for j in range(i,0,-1):
            if j<=n:
                qc.cp(-pi/2**(i-j),a[j-1],b[n+1-i])
    qc.barrier()
    qca_qftdagger(qc,b)
    
def qca_subber_swap(qc,a,b):
    #Input:
        #n+1 qubits of register indexed by a : (binary represantion in the computational basis of) a and |0\rangle on the last qubit
        #n+1 qubits of register indexed by b : |b\rangle on the first n qubits, |0\rangle on the last qubit (last qubit used to store last carry)
    #Output:
        #n qubits of register indexed by a : |a-b\rangle
        #n+1 qubits of register indexed by b : |b\rangle
    n=len(a)-1
    qca_qft(qc,a)
    qc.barrier()
    for i in range(n+1,0,-1):
        for j in range(i,0,-1):
            if j<=n:
                qc.cp(-pi/2**(i-j),b[j-1],a[n+1-i])
    qc.barrier()
    qca_qftdagger(qc,a)
    
#Division
#########

def qca_ls(qc,a,n):
    #left shifts register indexed by a n times.
    if(n==1):
        for i in range(len(a)-1):
            qc.swap(a[len(a)-1-i],a[len(a)-2-i])
    elif(n>1):
        for i in range(len(a)-1):
            qc.swap(a[len(a)-1-i],a[len(a)-2-i])
        qca_ls(qc,a,n-1)

def listrs(L):
    #Returns list obtained by right-shifting 1 time L
    return([L[-1]]+L[:-1])

def listls(L):
    #Returns list obtained by left-shifting 1 time L
    return(L[1:]+[L[0]])
        
def qca_divider_V2(qc,p,d,q):
    n=len(q)
    for i in range(n-1,-1,-1):
        # Multipliying |p\rangle by 2 : is equivalent to shifting it to the left by one unit
        qca_ls(qc,p,1)
        # Subtracting |d\rangle to |p\rangle, forcing rightmost qubit to be the subbing pad
        qca_subber_swap(qc,listls(p),listls(d))
        # If the result is positive, indicated by |p\rangle rightmost qubit being |0\rangle, then the i-th bit of the quotient is 1
        qc.x(p[0])
        qc.cx(p[0],q[i])
        qc.x(p[0])
        # Else, the result is negative and we add back |d\rangle to |p\rangle
        # This operation is controlled by the fact that the i-th qubit of the quotient is still |0\rangle
        qc.x(q[i])
        qca_c_adder(qc,listls(d),listls(p),q[i])
        qc.x(q[i])

def div(qc,p,d,q,n):
    qca_divider_V2(qc,p,d,q)

def qc_rotation (qc, n, control_qubit, target_qubit): 
    # qc : object quantum circuit 
    # n : number of qubits on which we control our rotation 
    # control_qubit : first position of controlled qubits
    # target_qubit : position of target qubit
    for i in range (n) : 
        qc.ry(1/2**(i+1), target_qubit)
        qc.cx(n+control_qubit-i-1 , target_qubit)
        qc.ry(-1/2**(i+1), target_qubit)
        qc.cx(n+control_qubit-i-1, target_qubit)
        
# for specific reasons, we would like to divide the controlled register in two parts 
# this is motivated by the fact that our implementation of division gives a quotient and rest
# and concerned qubits are located in two different regiters

def qc_rotation_bis (qc, n, control_qubit_1, control_qubit_2, target_qubit):
    # qc : object quantum circuit 
    # n : number of qubits divided by two on which we control our rotation 
    # control_qubit_1 : first position of first register of controlled qubits
    # control_qubit_2 : first position of second register of controlled qubits
    # target_qubit : position of target qubit
    for i in range (n) : 
        qc.ry(1/2**(i+1), target_qubit)
        qc.cx(n+control_qubit_1-i-1 , target_qubit)
        qc.ry(-1/2**(i+1), target_qubit)
        qc.cx(n+control_qubit_1-i-1, target_qubit)
    for i in range (n) : 
        qc.ry(1/2**(n+i+1), target_qubit)
        qc.cx(n+control_qubit_2-i-1 , target_qubit)
        qc.ry(-1/2**(n+i+1), target_qubit)
        qc.cx(n+control_qubit_2-i-1, target_qubit) 

qc = QuantumCircuit (4)
# init theta = |111>
qc.x(0) 
qc.x(1)
qc.x(2)
#qc.barrier()
qc_rotation (qc, 3, 0, 3)
qc.measure_all()
qc.draw()

backend = Aer.get_backend('qasm_simulator')
shots = 2048
results = execute(qc, backend=backend, shots=shots).result()
answer = results.get_counts()

plot_histogram(answer)

def HHL (num_ancille, num_state, t, matrix, initial_state, C=0) : 
    print(t)
    # num_ancillae is the number of qubits used in QPE for phase estimation (n_l)
    # num_state is the number of qubits used for |b> (n_b)
    # matrix is the matrix of the system
    # initial state is the value of vector b 
    # C parameter, if set 0 then C=|11...1> (biggest value possible but not a good ida, see remark)

        
    # 5*num_ancillae + num_state + 1 
    
    qr_theta = QuantumRegister(num_ancillae, name='theta')
    qr_b = QuantumRegister(num_state, name='b')
    qr_aux = QuantumRegister(num_ancillae, name='aux')
    qr_r = QuantumRegister(num_ancillae, name='r')
    qr_d = QuantumRegister(num_ancillae, name='d')
    qr_q = QuantumRegister(num_ancillae, name='q')
    qr_S = QuantumRegister(1, name='s')

    qc = QuantumCircuit (qr_theta, qr_b, qr_aux, qr_r, qr_d, qr_q, qr_S)


    # set the initial state using qiskit

    state_in = Custom(num_qubits=num_state, state_vector = np.array(initial_state))
    qc = qc.compose (state_in.construct_circuit('circuit', qr_b), [2])   #qc.data : Return the circuit data (instructions and context).

    # call the modules hamiltonian simulation (classical) + QPE + QFT
    global list_CU_operators, exponent_r, flag_get_CU_gate
    list_CU_operators = None
    exponent_r = None
    flag_get_CU_gate = None
    qpe_hhl(circuit=qc, pos_init_qreg_a=0, num_qubits_a=num_ancillae, pos_init_qreg_q=num_ancillae, unitary_matrix=None, matrix=matrix, t=t, measurement=False, Qiskit_reading_direction=True, nonzero_entries=nonzero_entries)

    qc.barrier()

    # call eigenvalue rotation 

    # step 1 : ASNINV

    for i in range (num_ancillae) : 
        qc.x(num_state+3*num_ancillae+i)

    #qc.x(num_state+4*num_ancillae-1)


    # Divider that takes |p>|d>|q>.
    # |p> is length 2n and has n zeros on the left: 0 ... 0 p_n ... p_1.
    # |d> has length 2n and has n zeros on the right: d_2n ... d_{n+1) 0 ... 0.
    # |q> has length n and is initially all zeros.
    # At the end of the algorithm, |q> will contain the quotient of p/d, and the
    # left n qubits of |p> will contain the remainder of p/d.

    #Qiskit order of qubits may be a little be confusing since qubits order are inverted meaning  
    # 0 ... 0 p_n ... p_1 will appear as |p_1 ... p_n 0 ... 0> 
    #

    p_idx = list (range(num_state+3*num_ancillae, num_state+4*num_ancillae)) + list (range(num_state+2*num_ancillae, num_state+3*num_ancillae))
    print("p_idx: ",p_idx)
    d_idx = list(range(num_state+num_ancillae,num_state+2*num_ancillae)) + list (range(num_ancillae))
    print("d_idx: ",d_idx)
    q_idx = list (range (num_state+4*num_ancillae, num_state+5*num_ancillae))
    print("q_idx: ",q_idx)
    
    div (qc=qc, 
         p=p_idx, 
         d=d_idx,
         q=q_idx, 
         n=num_ancillae)


    qc.barrier()


    # step 2 : controlled rotaiotn 

    qc_rotation_bis (qc, num_ancillae,  num_state+4*num_ancillae, num_state+2*num_ancillae,num_state+5*num_ancillae)

    qc.barrier()

    # step 3 : ASNINV_dagger

    qc_aux = QuantumCircuit (5*num_ancillae + num_state)

    div (qc_aux, list (range(num_state+3*num_ancillae, num_state+4*num_ancillae)) + list (range(num_state+2*num_ancillae, num_state+3*num_ancillae)), 
             list(range(num_state+num_ancillae,num_state+2*num_ancillae)) + list (range(num_ancillae)),
             list (range (num_state+4*num_ancillae, num_state+5*num_ancillae)), num_ancillae)

    qc = qc.compose (qc_aux.inverse(), list(range(num_state+ 5*num_ancillae)))

    for i in range (num_ancillae) : 
        qc.x(num_state+3*num_ancillae+i)

    #qc.x(num_state+4*num_ancillae-1)

    qc.barrier()

    # QPE dagger 

    qpedagger_hhl(circuit=qc, pos_init_qreg_a=0, num_qubits_a=num_ancillae, pos_init_qreg_q=num_ancillae, unitar_matrix=None, matrix=matrix, t=t, measurement=False, Qiskit_reading_direction=True, nonzero_entries=nonzero_entries)

    qc.barrier()
    #qc.draw(filename="nouveau")
    qc.measure_all()
    
    return qc



def liste_state (n_b, n_l) : 
    
    def liste_ket (n_b) : 
        if (n_b == 1) : 
            return ['0', '1']
        else : 
            L = liste_ket (n_b-1)
            res = []
            for x in L : 
                res.append (x+'1')
                res.append (x+'0')
            return res
        
    L = liste_ket (n_b)
    res = []
    for x in L : 
        res.append ((x,'1'+'0'*(4*n_l)+x+ '0'*n_l))
    return res
    
def analysis (num_state, num_ancillae, answer) : 
    nombre_1 = 0 
    nombre_tot = 0
    for cle, valeur in answer.items():
        nombre_tot += valeur
        if (str(cle)[0] == '1') : 
            nombre_1 += valeur
            
    print ("la probabilité de réussite de l'algorithme est d'environ", nombre_1*1.0/nombre_tot*100, "% de chance") 
    
    for x,y in liste_state (num_state, num_ancillae) :
        try:
            print ("la probabilité de mesurer l'état |"+ x +">", "est", answer[y]*1.0/nombre_1)
        except:
            pass

num_ancillae =   2       # n_l
num_state = 1            # n_b
t = 6*np.pi/8
matrix = [[1, -1/3],
          [-1/3, 1]]
initial_state = [1, 1]

qc = HHL (num_ancillae, num_state, t, matrix, initial_state)

# pour visualiser le circuit 
qc.draw (scale=0.55)

backend = Aer.get_backend('qasm_simulator')
shots = 2048*10
results = execute(qc, backend=backend, shots=shots).result()
answer = results.get_counts()

# calcul de la probabilité estimée de réussite de l'algorithme

analysis (num_state, num_ancillae, answer)

plot_histogram(answer)

num_ancillae =   2       # n_l
num_state = 1            # n_b
t = 6*np.pi/8          # t
matrix = [[1, -1/3],
          [-1/3, 1]]
initial_state = [1, 0]

qc = HHL (num_ancillae, num_state, t, matrix, initial_state)

# pour visualiser le circuit 
# qc.draw (scale=0.55)

backend = Aer.get_backend('qasm_simulator')
shots = 2048*10
results = execute(qc, backend=backend, shots=shots).result()
answer = results.get_counts()

# calcul de la probabilité estimée de réussite de l'algorithme

analysis (num_state, num_ancillae, answer)

plot_histogram(answer)

