def random_SVD_test(k, q):
	m = 100000
	n = 200

	U0, R = la.qr(np.random.randn(m, n))
	V, R = la.qr(np.random.randn(n, n))
	U = U0[:, 0:n]

	S = np.ones((n))
	t = 2.0

	j = 1
	while j < n:
		S[j] = 1/t
		t *= 2
		j += 1

	A = np.dot(U, np.dot(np.diag(S), V))

	U, S, V = random_SVD_iterated(A, k, k, q)
	dNormSigma = (la.norm(A-np.dot(U, np.dot(np.diag(S), V))), S[k+1])
	print dNormSigma

	U, S, V = random_SVD_fast(A, k, k)
	dNormSigma = (la.norm(A-np.dot(U, np.dot(np.diag(S), V))), S[k+1])
	print dNormSigma

def random_rot_matrix(D,c):
    A=npr.randn(D,D);
    P,R=qr(A);
    A=npr.randn(D,D);
    Q,R=qr(A);
    u=npr.rand(D);
    D=c**((u-np.min(u))/(np.max(u)-np.min(u)));
    D=diag(D);
    M=dot(P,dot(D,Q))
    return M

def principal_angle(A,B):
    """
    Find the principal angle between two subspaces
    spanned by columns of A and B
    """
    from numpy.linalg import qr, svd
    qA, _ = qr(A)
    qB, _ = qr(B)
    U,S,V = svd(qA.T.dot(qB))
    return np.arccos(min(S.min(), 1.0))

    def blockpower(self, ell, eps=1):
        n, d = self.getShape()
        init_mat = randn(d, ell)
        num_of_iter = int(10 * ceil(log(d / eps) / eps))

        for i in xrange(num_of_iter):
            [init_mat, _] = qr(init_mat)
            init_mat = self.covarianceMult(init_mat)

        K = self.leftMult(init_mat)
        [Q, _] = qr(K)
        del K
        del init_mat
        return Q

 def __subspace_iteration(self):
      A = self.A
      s = self.kwargs['s']
      q = self.kwargs['q']
      
      m, n = A.shape
      S = random.randn(n, s)
      Y = np.dot(A, S)
      Q, R = linalg.qr(Y)
      for i in range(q):
          Y = np.dot(A.T, Q)
          Q, R = linalg.qr(Y)
          Y = np.dot(A, Q)
          Q, R = linalg.qr(Y)
      return Q

def nullSpaceMethod(A, b, C, d):
    """
    Solves the constrained least squares problem min ||Ax-b||_2 subject to Cx=d via the null space method.
    :param numpy ndarray A: an m-by-n A matrix
    :param numpy ndarray b: an m-by-1 b matrix

    :return: x, the coefficients of the least squares problem.
    :rtype: ndarray
    :return: cond, the condition number of the final matrix on which least squares is performed
    :rtype: float
    """
    m, n = A.shape
    p, n = C.shape
    Q, R = qr(C.T, 'complete')
    #Q, R = qr_Householder(C.T)
    Q1 = Q[0:n, 0:p]
    Q2 = Q[0:n, p:n]
    # Lower triangular matrix!
    L = R.T
    L = L[0:p, 0:p]
    y1, not_required = solveLSQ(L, d)
    c = b - (A * Q1) * y1
    AQ2 = A * Q2
    y2, not_required = solveLSQ(AQ2 , np.mat(c) )
    x = (Q1 * y1) + (Q2 * y2)
    cond = np.linalg.cond(AQ2)
    return x, cond

def GMRES(A, b, krylovSize=10, useQR = True):
    def MultiplyMatrix(x):
        return dot(A, x)

    arnoldi = ArnoldiIterations(A, MultiplyMatrix, krylovSize)
    arnoldi.Setup(startVector = b)
    arnoldi.ArnoldiIterations()

    #converged = False
    #while not converged:
        #arnoldi step

        #check residual

    #Solve least square problem
    x = None
    bdStep = arnoldi.BreakdownStep
    if useQR:
        Q,R = linalg.qr(arnoldi.Hessenberg[:bdStep+1,:bdStep])
        Qb = dot(transpose(arnoldi.ArnoldiVectors[:,:bdStep+1]), b)
        Qbb = dot(transpose(Q), Qb)
        y = linalg.solve(R[:bdStep+1,:bdStep], Qbb)
        x = dot(arnoldi.ArnoldiVectors[:,:bdStep], y)
    else:
        HH = dot(transpose(arnoldi.Hessenberg), arnoldi.Hessenberg)
        bb = dot(transpose(arnoldi.Hessenberg), dot(transpose(arnoldi.ArnoldiVectors), b))
        y = linalg.solve(HH, bb)
        x = dot(arnoldi.ArnoldiVectors[:,:-1], y)

    return x

    def ComputeProjectionMatrix(self,B,Bcol,Glen):
        q,r=qr(B,mode='complete')
        P=q[:,Bcol:Glen]
#         if (not P):
#             print 'Projection matrix is empty'
#             exit()
        return P.T

def randomMatrices(n):
    """
    Save and return two lists of random rotation matrices and their inverse
    """
    from numpy.linalg import qr
    import cPickle
    rot = []
    inv = []
    for i in range(n):
        q, r = qr(np.random.randn(3,3))
        d = np.diagonal(r)
        d = d/np.abs(d)
        q = np.multiply(q,d)
        if np.linalg.det(q) < 0:
            q = np.fliplr(q) # make sure det > 0
        try:
            iq = np.linalg.inv(q)
        except: # in case of q is singular
            i -=1
            continue
        rot.append(q)
        inv.append(iq)
    t = (rot,inv)
    with open('_'.join(['rotation', str(n)]), 'wb') as f:
            cPickle.dump(t, f)
    return t

    def quadweights(self, eltid):
        """ return the quadrature weights on face faceid"""
        # The area of a simplex is 1/n! times the area of a parallepiped;
#        area = abs(nl.det(self.__mesh.directions[self.__mesh.etof[eltid][0]][1:]))# / math.factorial(self.__mesh.dim))
        area = abs(np.prod(nl.qr(self.__mesh.directions[self.__mesh.etof[eltid][0]][1:].T, mode='r').diagonal()))
        
        return self.__qw * area

def left_qr_maxvol(nd):
    cr = nd.core.copy()
    r1, n1, r2 = cr.shape
    cr = np.tensordot(nd.edges[0].Ru, cr, 1)
    #nd.edges[0].Ru = np.ones((1, 1))
    r1 = cr.shape[0]
    cr = reshape(cr, (r1 * n1, r2))
    q, Ru = qr(cr)
    ind, c = maxvol(q)
    Ru = np.dot(q[ind, :], Ru)
    q = c.copy()
    nd.core = reshape(q, (r1, n1, r2)).copy()
    nd.edges[1].Ru = Ru.copy()
    nd.maxvol_left = np.unravel_index(ind, (r1, n1), order='F')
    #The philosophical question if this index should be stored on the edge or in the node
    #The typical recomputation:
    #Take left index somewhere and update. For the first node it comes from the left edge
    #So, we can store ind_left on an edge, whereas ind_left_add in the node
    """ This is a logically separate function """
    i_left = nd.edges[0].ind_left
    #Update index
    w1 = mkron(np.ones((n1, 1), dtype=np.int32), i_left)
    w2 = mkron(reshape(np.arange(n1, dtype=np.int32),(-1, 1)), np.ones((r1, 1), dtype=np.int32))
    i_next = np.hstack((w1, w2))
    i_next = reshape(i_next, (r1 * n1, -1))
    i_next = i_next[ind, :]
    
    nd.edges[1].ind_left = i_next.copy()
    nd.edges[1].ind_left_add = i_next.copy()

def solve1():
    x = np.array([-1.02494, -0.949898, -0.866114,-0.773392, -0.671372,
                  -0.559524,-0.437067,-0.302909, -0.155493, -0.007464], dtype = "float_")
    y = np.array([-0.389269, -0.322894, -0.265256, -0.216557, -0.177152,
                  -0.147582, -0.128618, -0.121353, -0.127348,-0.148885], dtype = "float_")
    rhs = x**2
    col1 = y*y
    col2 = x*y
    col3 = x
    col4 = y
    col5 = np.ones(len(x))
    A = np.array([col1,col2,col3,col4,col5]).T
    Q, R = la.qr(A)
    Qt = Q.T
    c1 = np.dot(Qt, rhs)
    result = la.solve(R, c1)
    residual = rhs - np.dot(A, result)
    norm_residual = la.norm(residual, np.inf)
    print "Coefficients:"
    print result
    print "Residual vector: "
    print residual
    print "Residual inf norm:"
    print norm_residual
    return result

def comp(M):
    """Returns a basis for the space orthogonal
    to the range of M
    """
    I = eye(M.shape[0])
    Q,R = qr(concatenate((M,I),axis=1))
    return Q[:,matrix_rank(M):]

def Linsolve(A,b):
    """
    Solves the linear system A*x=b (if b is a column vector), or x*A=b (if b is 
    a row vector).
    
    Matrix "A" does not need to be square, this function uses rank-revealing
    QR decomposition to solve the system.
    
    Parameters
    ----------
    A : matrix, shape (M,N)
        The coefficient matrix of the linear system.
    b : matrix, shape (M,1) or (1,N)
        The right hand side of the linear system
        
    Returns
    -------
    x : matrix, shape (M,1) or (1,N)
        If b is a column vector, then x is the solution of A*x=b.       
        If b is a row vector, it returns the solution of x*A=b.
    """
    if b.shape[0]==1:
        x = Linsolve(np.conj(A.T), np.conj(b.T))
        return np.conj(x.T)
    elif b.shape[1]==1:
        Q,R = la.qr(A)
        N = A.shape[1]
        return ml.matrix(la.solve(R[0:N,0:N], np.array(np.conj(Q.T)*b).flatten()[0:N])).T

 def get_unitary(self):
     ''' build the unitary '''
     real=np.random.normal(0, 1, [self.nmodes, self.nmodes])
     imag=1j*np.random.normal(0, 1, [self.nmodes, self.nmodes])
     self.unitary=real+imag
     self.unitary, r = qr(self.unitary)
     return self.unitary

def main():
    from matplotlib.pyplot import figure,plot, close
    from numpy.random import standard_normal,choice
    from numpy.linalg import qr
    from numpy import dot
    import CAMP_C
    #from myOmp import omp_naive as omp
    N=2000
    M=900
    K=100
    sigma_n=0.001
    A=standard_normal((N,N))+1j*standard_normal((N,N))
    (Q,R)=qr(A)
    i=choice(N,M,False)  
    A=Q[i,:]

    x=(standard_normal((N,1))+1j*standard_normal((N,1)))/sqrt(2)
    j=choice(N,N-K,False)
    x[j,:]=0
    
    y=dot(A,x)+sigma_n*standard_normal((M,1))
    xhat=CAMP_C.CAMP(A,y,1,True)
    print norm(x-xhat)/N
    close('all')
    plot(real(x))
    plot(real(xhat))
    figure()
    plot(imag(x))
    plot(imag(xhat))

def test_nystrom_extension(seed=123):
    """ Test Nystrom Extension: low rank approximation is exact when
    G is itself low rank
    """
    n = 5
    s = 2
    rng = np.random.RandomState(seed)
    X = rng.randn(n, s)
    G = np.dot(X, X.T) # has rank s

    # find the linearly independent columns of 
    q = qr(G)[1] 
    q = absolute(q)
    sums = np.sum(q,axis=1)
    i = 0
    dims = list()
    while( i < n ): #dim is the matrix dimension
        if(sums[i] > 1.e-10):
            dims.append(i)
        i += 1
    
    # Find the eigendecomposition of the full rank portion:
    W = G[dims,:]
    W = W[:,dims]
    eval, evec = np.linalg.eigh(W)
    
    # pass the dims columns of G 
    C = G[:,dims]
    # Find the estimated eigendecomposition using Nystrom 
    eval_nystrom, evec_nystrom = nystrom_extension(C, evec, eval)
        
    # reconstruct G using Nystrom Approximatiuon 
    G_nystrom = np.dot(np.dot(evec_nystrom, np.diag(eval_nystrom)),evec_nystrom.T)
    # since rank(W) = rank(G) = s the nystrom approximation of G is exact:
    assert_array_almost_equal(G_nystrom, G)

 def initBeforeMake(self, dimension, \
                    signal_dimension=0, \
                    signal_to_noise_ratio=0,\
                    signal_singular_value_decay_factor=0, \
                    signal_singular_value_decay_type='exp'):
     
     self.dimension = dimension
     self.signal_dimension = signal_dimension
     self.signal_to_noise_ratio = signal_to_noise_ratio
     self.signal_singular_value_decay_factor = signal_singular_value_decay_factor
     self.signal_singular_value_decay_type = signal_singular_value_decay_type
 
     # setting a random singular space    
     [Q,R] = qr( randn(self.dimension, self.signal_dimension) )
     self.signal_row_space = Q.transpose()
     del Q,R
     
     # setting the singular values  
     eta = self.signal_singular_value_decay_factor
     if self.signal_singular_value_decay_type == 'exp':
         self.signal_singular_values = [exp(-10*eta*i/self.signal_dimension) for i in xrange(self.signal_dimension)] 
     elif self.signal_singular_value_decay_type == 'lin':
         self.signal_singular_values = [max(1.0 - eta*float(i)/self.signal_dimension,0.0) for i in xrange(self.signal_dimension)]
     else:
         self.signal_singular_values = ones(self.signal_dimension)
     # done initializing 
     self.wasInitForMake = True

 def init(self, n_in, n_out):
     W = self.rng.randn(n_in, n_out, *args, **kwargs)
     trans = False
     if n_in < n_out:
         W = W.T
         trans = True
     W, _ = linalg.qr(W)
     ret_W = W.T if trans else W
     return numpy.asarray(ret_W, dtype='float32')

 def __fast_generic(self):
     A = self.A
     s = self.kwargs['s']
     
     m, n = A.shape
     SRFT = SRFT_matrix(n, s)
     Y = np.dot(A, SRFT)
     Q, R = linalg.qr(Y)
     return Q