def func(self, X, V):
        H = self.F.sysfunc.hess(X, self.F.coords, self.F.coords)
        q = self.F.testfunc.data.v/linalg.norm(self.F.testfunc.data.v)
        p = self.F.testfunc.data.w/matrixmultiply(transpose(self.F.testfunc.data.w),q)

        return array(0.5*matrixmultiply(transpose(p), reshape([bilinearform(H[i,:,:], q, q) \
             for i in range(H.shape[0])],(H.shape[0],1)))[0])

    def __pade_approximation(self, nodeName, L, M, debug=0):
        assert L == M - 1
        num_moments = L+M+1
        if debug: "DATA L=%s M=%s num_moments=%s" % (L, M, num_moments)

        if debug: print "num_moments", num_moments
        #scaling = 766423.0
        node_index = self.TranslateNode(nodeName)
        node_moments = []
        # step 1: calculate the Moments
        g_inverse = numpy.linalg.inverse(self.G)
        if debug: print "g_inverse", g_inverse
        last_moment = numpy.matrixmultiply(g_inverse, self.B)
        if debug: print "last_moment", last_moment, node_index
        node_moments.append(last_moment[node_index-1][0])

		# test commit
        for i in range(num_moments-1):
            intermediate = -1 * numpy.matrixmultiply(g_inverse, self.C)
            last_moment = numpy.matrixmultiply( intermediate, last_moment )
            moment = self.Scaling * last_moment[node_index-1][0]
            node_moments.append(moment)
            last_moment = self.Scaling * last_moment
            if debug: print "last_moment", last_moment

        print "suggested scaling =", node_moments[0]/node_moments[1]
        if debug: print "node_moments=", node_moments

        # Call the general pade algorithm
        a_coef, b_coef = MathHelpers.my_pade(node_moments, complex=False)

        # Return results
        return a_coef, b_coef, node_moments

def bCorrNumeric(X, Y, DX, beat_input, cut=0.001, app=0, path="./"):
    R = np.transpose(beat_input.sensitivity_matrix)
    b = beat_input.computevectorEXP(X, Y, DX) - beat_input.zerovector
    corr = beat_input.varslist
    m, n = np.shape(R)
    if len(b) == m and len(corr) == n:
        rms, ptop = calcRMSNumeric(b)
        inva = generalized_inverse(R, cut)
        print "initial {RMS, Peak}: { %e , %e } mm" % (rms, ptop)
        print "finding best over", n, "correctors"
        for i in range(n):
            dStren = matrixmultiply(inva[i, :], b)
            bvec = b - matrixmultiply(R[:, i], dStren)
            rm, ptp = calcRMSNumeric(bvec)
            if rm < rms:
                rms = rm
                rbest = i
                rStren = dStren
            if ptp < ptop:
                ptop = ptp
                pbest = i
                pStren = dStren
        print "final {RMS, Peak}: { %e , %e }" % (rms, ptop)
        if rbest == pbest:
            print "best corr:", corr[rbest], '%e' % (rStren), "1/m"
        else:
            print "--- warning: best corr for rms & peak are not same"
            print "RMS best corr:", corr[rbest], '%e' % (rStren), "1/m"
            print "Peak best corr:", corr[pbest], '%e' % (pStren), "1/m"
    else:
        print "dimensional mismatch in input variables"

def intersect_line_ellipsoid(p1, p2, ec, radius_vectors):
    """Determine intersection points between line defined by p1 and p2,
    and ellipsoid defined by centre ec and three radius vectors (tuple
    of tuples, each inner tuple is a radius vector).

    This requires numpy.
    """


    # create transformation matrix that has the radius_vectors
    # as its columns (hence the transpose)
    rv = numpy.transpose(numpy.matrix(radius_vectors))
    # calculate its inverse
    rv_inv = numpy.linalg.pinv(rv)
    
    # now transform the two points
    # all points have to be relative to ellipsoid centre
    # the [0] at the end and the numpy.array at the start is to make sure
    # we pass a row vector (array) to the line_sphere_intersection
    p1_e = numpy.array(numpy.matrixmultiply(rv_inv, numpy.array(p1) - numpy.array(ec)))[0]
    p2_e = numpy.array(numpy.matrixmultiply(rv_inv, numpy.array(p2) - numpy.array(ec)))[0]

    # now we only have to determine the intersection between the points
    # (now transformed to ellipsoid space) with the unit sphere centred at 0
    isects_e = intersect_line_sphere(p1_e, p2_e, (0.0,0.0,0.0), 1.0)

    # transform intersections back to "normal" space
    isects = []
    for i in isects_e:
        # numpy.array(...)[0] is for returning only row of matrix as array
        itemp = numpy.array(numpy.matrixmultiply(rv, numpy.array(i)))[0]
        isects.append(itemp + numpy.array(ec))

    return isects

    def updatedata(self, A):
        # Update b, c
        try:
            ALU = linalg.lu_factor(A)
            BC = linalg.lu_solve(ALU, c_[linalg.lu_solve(ALU, self.data.b + 1e-8*self.data.Brand[:,:1]), \
                                 self.data.c + 1e-8*self.data.Crand[:,:1]], trans=1)
            C = linalg.lu_solve(ALU, BC[:,-1:])
            B = BC[:,:1]
        except:
            if self.C.verbosity >= 1:
                print 'Warning: Problem updating border vectors.  Using svd...'
            U, S, Vh = linalg.svd(A)
            B = U[:,-1:]
            C = transpose(Vh)[:,-1:]

        bmult = cmult = 1
        if matrixmultiply(transpose(self.data.b), B) < 0:
            bmult = -1
        if matrixmultiply(transpose(self.data.c), C) < 0:
            cmult = -1
        self.data.b = bmult*B*(linalg.norm(A,1)/linalg.norm(B))
        self.data.c = cmult*C*(linalg.norm(A,Inf)/linalg.norm(C))

        # Update
        if self.update:
            self.data.B[:,0] = self.data.b*(linalg.norm(A,1)/linalg.norm(self.data.b))
            self.data.C[:,0] = self.data.c*(linalg.norm(A,Inf)/linalg.norm(self.data.c))

            self.data.B[:,1] = self.data.w[:,2]*(linalg.norm(A,1)/linalg.norm(self.data.w,1))
            self.data.C[:,1] = self.data.v[:,2]*(linalg.norm(A,Inf)/linalg.norm(self.data.v,1))

            self.data.D[0,1] = self.data.g[0,1]
            self.data.D[1,0] = self.data.g[1,0]

    def func(self, X, V):
        k = self.C.TFdata.k
        v1 = self.C.TFdata.v1
        w1 = self.C.TFdata.w1

        if k >=0:
            J_coords = self.F.sysfunc.J_coords
            w = sqrt(k)

            q = v1 - (1j/w)*matrixmultiply(self.F.sysfunc.J_coords,v1)
            p = w1 + (1j/w)*matrixmultiply(transpose(self.F.sysfunc.J_coords),w1)

            p /= linalg.norm(p)
            q /= linalg.norm(q)

            p = reshape(p,(p.shape[0],))
            q = reshape(q,(q.shape[0],))

            direc = conjugate(1/matrixmultiply(transpose(conjugate(p)),q))
            p = direc*p

            l1 = firstlyapunov(X, self.F.sysfunc, w, J_coords=J_coords, p=p, q=q)

            return array([l1])
        else:
            return array([1])

    def process(self, X, V, C):
        BifPoint.process(self, X, V, C)

        # Finds the new branch
        J_coords = C.CorrFunc.jac(X, C.coords)
        J_params = C.CorrFunc.jac(X, C.params)
        A = r_[c_[J_coords, J_params], [V]]
        W, VR = linalg.eig(A)
        W0 = [ind for ind, eig in enumerate(W) if abs(eig) < 5e-5]
        V1 = real(VR[:,W0[0]])

        H = C.CorrFunc.hess(X, C.coords+C.params, C.coords+C.params)
        c11 = matrixmultiply(self.data.psi,[bilinearform(H[i,:,:], V, V) for i in range(H.shape[0])])
        c12 = matrixmultiply(self.data.psi,[bilinearform(H[i,:,:], V, V1) for i in range(H.shape[0])])
        c22 = matrixmultiply(self.data.psi,[bilinearform(H[i,:,:], V1, V1) for i in range(H.shape[0])])

        beta = 1
        alpha = -1*c22/(2*c12)
        V1 = alpha*V + beta*V1
        V1 /= linalg.norm(V1)

        self.found[-1].eigs = W
        self.found[-1].branch = todict(C, V1)

        self.info(C, -1)

        return True

    def updatedata(self, A):
        if self.update:
            if self.corr:
                B = self.data.w
                C = self.data.v
            else:
                # Note: Problem when singular vectors switch smallest singular value (See NewLorenz).
                #       To overcome this, I have implemented a 1e-8 random nudge.
                try:
                    ALU = linalg.lu_factor(A)
                    # BC = linalg.lu_solve(ALU, c_[linalg.lu_solve(ALU, self.data.B), self.data.C], trans=1)
                    # USE OF RANDOM NUDGE
                    BC = linalg.lu_solve(ALU, c_[linalg.lu_solve(ALU, self.data.B + 1e-8*self.data.Brand), \
                                                 self.data.C + 1e-8*self.data.Crand], trans=1)
                    C = linalg.lu_solve(ALU, BC[:,-1*self.data.q:])
                    B = BC[:,0:self.data.p]
                except:
                    if self.C.verbosity >= 1:
                        print('Warning: Problem updating border vectors.  Using svd...')
                    U, S, Vh = linalg.svd(A)
                    B = U[:,-1*self.data.p:]
                    C = transpose(Vh)[:,-1*self.data.q:]

            bmult = cmult = 1
            if matrixmultiply(transpose(self.data.B), B) < 0:
                bmult = -1
            if matrixmultiply(transpose(self.data.C), C) < 0:
                cmult = -1
            self.data.B = bmult*B*(linalg.norm(A,1)/linalg.norm(B))
            self.data.C = cmult*C*(linalg.norm(A,Inf)/linalg.norm(C))

 def calc_orth_symop(self, symop):
     """Calculates the orthogonal space symmetry operation (return SymOp)
     given a fractional space symmetry operation (argument SymOp).
     """
     RF  = numpy.matrixmultiply(symop.R, self.orth_to_frac)
     ORF = numpy.matrixmultiply(self.frac_to_orth, RF)
     Ot  = numpy.matrixmultiply(self.frac_to_orth, symop.t)
     return SpaceGroups.SymOp(ORF, Ot)

    def __locate_newton(self, X, C):
        """x[0:self.dim] = (x,alpha)
           x[self.dim] = beta
           x[self.dim+1:2*self.dim] = p
        """
        J_coords = C.CorrFunc.jac(X[0:C.dim], C.coords)
        J_params = C.CorrFunc.jac(X[0:C.dim], C.params)

        return r_[C.CorrFunc(X[0:C.dim]) + X[C.dim]*X[C.dim+1:], \
                  matrixmultiply(transpose(J_coords),X[C.dim+1:]), \
                  matrixmultiply(transpose(X[C.dim+1:]),J_params), \
                  matrixmultiply(transpose(X[C.dim+1:]),X[C.dim+1:]) - 1]

    def do_svd_clean(self, plane):
        """Does a SVD clean on the given plane"""
        time_start = time.time()
        print "(plane {0}) removing noise floor with SVD".format(plane)

        A = numpy.array(self.sddsfile.dictionary_plane_to_bpms[plane].bpm_data)
        number_of_bpms = A.shape[0]

        if number_of_bpms <= 10:
            sys.exit("Number of bpms <= 10")

        # normalise the matrix
        sqrt_number_of_turns = numpy.sqrt(A.shape[1])
        A_mean = numpy.mean(A)
        A = (A - A_mean) / sqrt_number_of_turns

        if PRINT_TIMES:
            print ">> Time for svdClean (before SVD call): {0}s".format(time.time() - time_start)
        USV = self.get_singular_value_decomposition(A)
        if PRINT_TIMES:
            print ">> Time for svdClean (after SVD call): {0}s".format(time.time() - time_start)

        # remove bad BPM by SVD -tbach
        good_bpm_indices = self.remove_bad_bpms_and_get_good_bpm_indices(USV, plane)
        USV = (USV[0][good_bpm_indices], USV[1], USV[2])
        number_of_bpms = len(good_bpm_indices)
        print ">> Values in GOOD BPMs: "
        print number_of_bpms

        # ----SVD cut for noise floor
        if _InputData.singular_values_amount_to_keep < number_of_bpms:
            print "(plane {0}) amount of singular values to keep: {1}".format(
                plane, _InputData.singular_values_amount_to_keep
            )
            USV[1][_InputData.singular_values_amount_to_keep :] = 0
        else:
            print "requested more singular values than available(={0})".format(number_of_bpms)

        A = matrixmultiply(USV[0], matrixmultiply(numpy.diag(USV[1]), USV[2]))
        # A0 * (A1 * A2) should require less operations than (A0 * A1) * A2,
        #  because of the different sizes
        # A0 has (M, K), A1 has (K, K) and A2 has (K, N) with K=min(M,N)
        # Most of the time, the number of turns is greater then
        #  the number of BPM, so M > N
        # --tbach
        A = (A * sqrt_number_of_turns) + A_mean
        bpmres = numpy.mean(numpy.std(A - self.sddsfile.dictionary_plane_to_bpms[plane].bpm_data, axis=1))
        print "(plane {0}) Average BPM resolution: ".format(plane) + str(bpmres)
        self.sddsfile.dictionary_plane_to_bpms[plane].bpm_data = A
        if PRINT_TIMES:
            print ">> Time for do_svd_clean: {0}s".format(time.time() - time_start)

def test_module():
    import random
    import AtomMath
    import FileIO
    import Structure
    
    R = AtomMath.rmatrixu(numpy.array((0.0, 0.0, 1.0), float), math.pi/2.0)

    struct1 = FileIO.LoadStructure(fil="/home/jpaint/8rxn/8rxn.pdb")
    struct2 = FileIO.LoadStructure(fil="/home/jpaint/8rxn/8rxn.pdb")

    chn1 = struct1.get_chain("A")
    chn2 = struct2.get_chain("A")

    rc = lambda: 0.1 * (random.random() - 1.0)
    for atm in chn2.iter_atoms():
        atm.position = numpy.matrixmultiply(R, atm.position) + numpy.array((rc(),rc(),rc()),float)
        
    alist = []
 
    for atm1 in chn1.iter_atoms():
        if atm1.name != "CA":
            continue

        atm2 = chn2.get_equivalent_atom(atm1)
        if atm2 == None: continue

        alist.append((atm1, atm2))

    sup = SuperimposeAtoms(alist)

    R = sup.R
    Q = sup.Q
    print Q
    print R
    
    so = sup.src_origin
    do = sup.dst_origin
    
    sup1 = Structure.Structure(structure_id = "JMP1")
    for atm in chn1.iter_atoms():
        atm.position = numpy.matrixmultiply(R, atm.position - so)
        sup1.add_atom(atm)
    FileIO.SaveStructure(fil="super1.pdb", struct=sup1)

    sup2 = Structure.Structure(structure_id = "JMP2")
    for atm in chn2.iter_atoms():
        atm.position = atm.position - do
        sup2.add_atom(atm)
    FileIO.SaveStructure(fil="super2.pdb", struct=sup2)

def stdForm(a, b):
    def invert(L):  # inverts lower triangular matrix L
        n = len(L)
        for j in range(n - 1):
            L[j, j] = 1.0 / L[j, j]
            for i in range(j + 1, n):
                L[i, j] = -dot(L[i, j:i], L[j:i, j]) / L[i, i]
        L[n - 1, n - 1] = 1.0 / L[n - 1, n - 1]

    n = len(a)
    L = choleski(b)  # COMEBACKTO!!!
    invert(L)
    h = matrixmultiply(b, matrixmultiply(a, transpose(L)))
    return h, transpose(L)

    def Cmatrix(self):
        '''
         Calculate the C matrix
        '''
        self.C = []
        self.gamma = []
        self.f1001 = []
        self.f1010 = []
        S = getattr(self, "S")
        R11 = getattr(self, "R11")
        R12 = getattr(self, "R12")
        R21 = getattr(self, "R21")
        R22 = getattr(self, "R22")
        BETX = getattr(self, "BETX")
        BETY = getattr(self, "BETY")
        ALFX = getattr(self, "ALFX")
        ALFY = getattr(self, "ALFY")

        J = numpy.reshape(numpy.array([0, 1, -1, 0]), (2, 2))
        for j in range(0, len(S)):
            R = numpy.array([[R11[j], R12[j]], [R21[j], R22[j]]])
            
            C = matrixmultiply(-J, matrixmultiply(numpy.transpose(R), J))
            C = (1 / numpy.sqrt(1 + determinant(R))) * C

            g11 = 1 / numpy.sqrt(BETX[j])
            g12 = 0
            g21 = ALFX[j] / numpy.sqrt(BETX[j])
            g22 = numpy.sqrt(BETX[j])
            Ga = numpy.reshape(numpy.array([g11, g12, g21, g22]), (2, 2))

            g11 = 1 / numpy.sqrt(BETY[j])
            g12 = 0
            g21 = ALFY[j] / numpy.sqrt(BETY[j])
            g22 = numpy.sqrt(BETY[j])
            Gb = numpy.reshape(numpy.array([g11, g12, g21, g22]), (2, 2))
            C = matrixmultiply(Ga, matrixmultiply(C, inverse(Gb)))
            gamma = 1 - determinant(C)
            self.gamma.append(gamma)
            C = numpy.ravel(C)
            self.C.append(C)
            self.f1001.append(((C[0] + C[3]) * 1j + (C[1] - C[2])) / 4 / gamma)
            self.f1010.append(((C[0] - C[3]) * 1j + (-C[1] - C[2])) / 4 / gamma)

        self.F1001R = numpy.array(self.f1001).real
        self.F1001I = numpy.array(self.f1001).imag
        self.F1010R = numpy.array(self.f1010).real
        self.F1010I = numpy.array(self.f1010).imag
        self.F1001W = numpy.sqrt(self.F1001R ** 2 + self.F1001I ** 2)
        self.F1010W = numpy.sqrt(self.F1010R ** 2 + self.F1010I ** 2)

    def svdClean(self, plane):
        global nturns, tx, ty
        print 'removing noise floor',plane
        
        if plane == 'x':
            b = tx[turn:,:]  #truncate by the first 5 turns
            n_turns = shape(b)[0]

        elif plane == 'y':
            b = ty[turn:,:]  #truncate by the first 5 turns
            n_turns = shape(b)[0]
        else:
            print "no tbt data acquired"
        
        b_mean = mean(b)
        b = (b-b_mean)/sqrt(n_turns)
        n_bpms = shape(b)[1]
        #----svd for matrix with bpms >10

        if n_bpms > 10:
            A = singular_value_decomposition(b,full_matrices=0)
            #print "Singular values:",A[1]
        else:
            sys.exit('Exit, # of bpms < 10')
        
        
        #----SVD cut for noise floor
        if sing_val > n_bpms:
            svdcut = n_bpms
            print 'requested more singular values than available'
            print '# of sing_val used for', plane, '=', n_bpms
        else:
            svdcut = int(sing_val)
            print '# of sing_val used for', plane, '=', svdcut
        #print A[1][0]
	A[1][svdcut:] = 0.
       	#temp=matrixmultiply(identity(len(A[1]))*A[1], A[2])
	temp=matrixmultiply(diag(A[1]), A[2])
	b = matrixmultiply(A[0],temp) ### check
        b = (b *sqrt(n_turns))+b_mean
        #b = b*sqrt(n_turns)
        
        if plane == 'x':
            tx[turn:,:] = b
        elif plane == 'y':
            ty[turn:,:] = b
        else:
            print "no tbt data to analyze"
        nturns = shape(tx)[0]

def bialttoeig(q, p, n, A):
    v1, v2 = invwedge(q, n)
    w1, w2 = invwedge(p, n)

    A11 = bilinearform(A,v1,v1)
    A22 = bilinearform(A,v2,v2)
    A12 = bilinearform(A,v1,v2)
    A21 = bilinearform(A,v2,v1)
    v11 = matrixmultiply(transpose(v1),v1)
    v22 = matrixmultiply(transpose(v2),v2)
    v12 = matrixmultiply(transpose(v1),v2)
    D = v11*v22 - v12*v12
    k = (A11*A22 - A12*A21)/D

    return k[0][0], v1, w1

    def process(self, X, V, C):
        BifPoint.process(self, X, V, C)

        # Finds the new branch
        J_coords = C.CorrFunc.jac(X, C.coords)
        J_params = C.CorrFunc.jac(X, C.params)


        singular = True
        perpvec = r_[1,zeros(C.dim-1)]
        d = 1
        while singular and d <= C.dim:
            try:
                v0 = linalg.solve(r_[c_[J_coords, J_params],
                                  [perpvec]], \
                                  r_[zeros(C.dim-1),1])
            except:
                perpvec = r_[0., perpvec[0:(C.dim-1)]]
                d += 1
            else:
                singular = False

        if singular:
            raise PyDSTool_ExistError("Problem in _compute: Failed to compute tangent vector.")
        v0 /= linalg.norm(v0)
        V = sign([x for x in v0 if abs(x) > 1e-8][0])*v0

        A = r_[c_[J_coords, J_params], [V]]
        W, VR = linalg.eig(A)
        W0 = [ind for ind, eig in enumerate(W) if abs(eig) < 5e-5]
        V1 = real(VR[:,W0[0]])

        H = C.CorrFunc.hess(X, C.coords+C.params, C.coords+C.params)
        c11 = matrixmultiply(self.data.psi,[bilinearform(H[i,:,:], V, V) for i in range(H.shape[0])])
        c12 = matrixmultiply(self.data.psi,[bilinearform(H[i,:,:], V, V1) for i in range(H.shape[0])])
        c22 = matrixmultiply(self.data.psi,[bilinearform(H[i,:,:], V1, V1) for i in range(H.shape[0])])

        beta = 1
        alpha = -1*c22/(2*c12)
        V1 = alpha*V + beta*V1
        V1 /= linalg.norm(V1)

        self.found[-1].eigs = W
        self.found[-1].branch = todict(C, V1)

        self.info(C, -1)

        return True

def eigenvector_for_largest_eigenvalue(matrix):
    """Returns eigenvector corresponding to largest eigenvalue of matrix.
    
    Implements a numerical method for finding an eigenvector by repeated 
    application of the matrix to a starting vector. For a matrix A the 
    process w(k) <-- A*w(k-1) converges to eigenvector w with the largest 
    eigenvalue. Because distance matrix D has all entries >= 0, a theorem 
    due to Perron and Frobenius on nonnegative matrices guarantees good 
    behavior of this method, excepting degenerate cases where the 
    iteration oscillates. For distance matrices a remedy is to add the 
    identity matrix to A, permitting the iteration to converge to the 
    eigenvector. (From Sander and Schneider (1991), and Vingron and 
    Sibbald (1993)) 
    
    Note: Only works on square matrices.
    """
    #always add the identity matrix to avoid oscillating behavior
    matrix = matrix + identity(len(matrix))

    #v is a random vector (chosen as the normalized vector of ones)
    v = ones(len(matrix))/len(matrix)

    #iterate until convergence
    for i in range(1000):
        new_v = matrixmultiply(matrix,v)
        new_v = new_v/sum(new_v, 0) #normalize
        if sum(list(map(abs,new_v-v)), 0) > 1e-9:
            v = new_v #not converged yet
            continue
        else: #converged
            break
    
    return new_v

def numeric_gemm_var1_flat(A, B, C, mc, kc, nc, mr=1, nr=1):
  M, N = C.shape
  K = A.shape[0]

  mc = min(mc, M)
  kc = min(kc, K)
  nc = min(nc, N)  

  tA = numpy.zeros((mc, kc), dtype = numpy.float)
  tB = numpy.zeros((kc, N), dtype = numpy.float)  

  for k in range(0, K, kc):
    # Pack B into tB
    tB[:,:] = B[k:k+kc:,:]
    
    for i in range(0, M, mc):
      imc = i+mc
      # Pack A into tA
      tA[:,:] = A[i:imc,k:k+kc]
      
      for j in range(0, N): # , nc):
        # Cj += ABj + Cj
        # jnc = j+nc
        ABj = numpy.matrixmultiply(tA, tB[:,j])
        numpy.add(C[i:imc:,j], ABj, C[i:imc:,j])
        
        # Store Caux into memory
  return

 def classify(self, vector):
     if self._should_normalise:
         vector = self._normalise(vector)
     if self._Tt != None:
         vector = numpy.matrixmultiply(self._Tt, vector)
     cluster = self.classify_vectorspace(vector)
     return self.cluster_name(cluster)