def compDklgaussian(mu1, C1, mu2, C2):
    '''
    mu1 and C1 specify a multidimensional gaussian. mu2 and C2 specify another
    one (of the same dimension). Return is a float giving the KL divergence
    between the two Gaussians.

    Inputs have to be matrix instances, and the mu inputs should be in row
    vector shape (mu.shape[0] = 1, mu.shape[1] > 1)

    '''
    n = mu1.size
    b = mu2 - mu1
    C2inv = la.inv(C2)
    C1sqrt = np.mat(sqrtPSDm(C1))
    Term1 = C1sqrt * C2inv * C1sqrt
    Term2 = b.transpose() * C2inv * b
    det1 = la.det(C1)
    det2 = la.det(C2)
    tol = 1e8
    if (la.cond(C1) > tol) | (la.cond(C2) > tol):
        print('Determinants not non-zero. Ignoring determinants.')
        Term3 = 0
    else:
        Term3 = .5 * np.log(det2 / det1)
    d = .5 * np.trace(Term1) + .5 * Term2 - .5 * n + Term3
    return d[0, 0]

def _B2formula(Ac, t1, t2, B2):
    if t1 == 0 and t2 == 0:
        term = B2
        return term
    n = Ac.shape[0]
    tmp = np.eye(n) - expm(-Ac)
    if cond(tmp) < 1000000.0:
        term = np.dot(((expm(-Ac * t1) - expm(-Ac * t2)) * inv(tmp)), B2)
        return term
    # Numerical trouble. Perturb slightly and check the result
    ntry = 0
    k = np.sqrt(eps)
    Ac0 = Ac
    while ntry < 2:
        Ac = Ac0 + k * rand(n, n)
        tmp = np.eye(n) - expm(-Ac)
        if cond(tmp) < 1 / np.sqrt(eps):
            ntry = ntry + 1
            if ntry == 1:
                term = np.dot(np.dot(expm(-Ac * t1) - expm(-Ac * t2), inv(tmp)), B2)
            else:
                term1 = np.dot(np.dot(expm(-Ac * t1) - expm(-Ac * t2), inv(tmp)), B2)
        k = k * np.sqrt(2)

    if norm(term1 - term) > 0.001:
        warn("Inaccurate calculation in mapCtoD.")
    return term

 def update(self, dt):
     self.mass_cond.append(linalg.cond(self._world._mass))
     self.imp_cond.append(linalg.cond(self._world._impedance))
     curr_rk = []
     for b in self._world.iterbodies():
         curr_rk.append( self.rank(b.jacobian) )
     self.c_rank.append(curr_rk)

def main(argv):
    A = numpy.matrix([[float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3]), float(sys.argv[4])], [float(sys.argv[5]), float(sys.argv[6]), float(sys.argv[7]), float(sys.argv[8])], [float(sys.argv[9]), float(sys.argv[10]), float(sys.argv[11]), float(sys.argv[12])]])
    B = numpy.matrix([[float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3])], [float(sys.argv[5]), float(sys.argv[6]), float(sys.argv[7])], [float(sys.argv[9]), float(sys.argv[10]), float(sys.argv[11])]])
    #print A
    #print 'top left number is: '+ str(A[0,0])
    #backSub(forwardElim(A))
    print LA.cond(B, numpy.inf)

def evaluate_rotation_matrix(R, this_theta):
    pi = numpy.pi
    print
    print '###################'
    theta = this_theta * 180.0 / pi
    condition_number = linalg.cond(R)
    smallest_singular_value = linalg.cond(R, -2)
    two_norm = linalg.cond(R, 2)
    print 'theta = ', theta, '\n', R, '\ncondition number = ', condition_number
    print 'largest_singluar_value = ', two_norm
    print 'smallest_singular_value = ', smallest_singular_value
    print '###################'
    print

    return

    def computeRegressorLinDepsSVD(self):
        """get base regressor and identifiable basis matrix with iDynTree (SVD)"""

        with helpers.Timer() as t:
            # get subspace basis (for projection to base regressor/parameters)
            Yrand = self.getRandomRegressor(5000)
            #A = iDynTree.MatrixDynSize(self.num_params, self.num_params)
            #self.generator.generate_random_regressors(A, False, True, 2000)
            #Yrand = A.toNumPy()
            U, s, Vh = la.svd(Yrand, full_matrices=False)
            r = np.sum(s>self.opt['min_tol'])
            self.B = -Vh.T[:, 0:r]
            self.num_base_params = r

            print("tau: {}".format(self.tau.shape), end=' ')

            print("YStd: {}".format(self.YStd.shape), end=' ')
            # project regressor to base regressor, Y_base = Y_std*B
            self.YBase = np.dot(self.YStd, self.B)
            if self.opt['verbose']:
                print("YBase: {}, cond: {}".format(self.YBase.shape, la.cond(self.YBase)))

            self.num_base_params = self.YBase.shape[1]
        if self.showTiming:
            print("Getting the base regressor (iDynTree) took %.03f sec." % t.interval)

 def EV(sasb_n, sasb_nm1, sa_n, sa_nm1,debug1=False,debug2=False):
     try:
         dim = len(sa_n)
         sasb_n  =  sasb_n.reshape(dim , dim)
         sasb_nm1=sasb_nm1.reshape(dim , dim)
         dsdk_n  =sasb_n  -outer(sa_n,sa_n)
         dsdk_nm1=sasb_nm1-outer(sa_nm1,sa_n)
         T=dot(dsdk_nm1,inv(dsdk_n))
         if debug2:
             print(cond(T))
         #evs=sort(abs(eigvals(T)))
         evs=sort(eigvals(T))
         if debug1:
             val,vec=eig(T)
             print(val)
             print('-----------------')
             for i in range(len(val)):
                 print(val[i],vec[:,i])
             print('-----------------')
         lambda_ =max(abs(evs))
         lambda_test =evs[-1]
         #assert(lambda_==lambda_test)
         if(len(evs)>1):
             lambda_2=evs[-2]
         else:
             lambda_2=0
         lambda_=RawSanitize(lambda_)
         return lambda_, lambda_2
     except np.linalg.linalg.LinAlgError:
         return 0,0

def unteraufgabe_d():
    n = np.arange(2,21,2)
    xs = [x02,x04,x06,x08,x10,x12,x14,x16,x18,x20]
    f = lambda x: np.sin(10*x*np.cos(x))

    residuals = np.zeros_like(n,dtype=np.floating)
    condition = np.ones_like(n,dtype=np.floating)

    for i, x in enumerate(xs):
        b = f(x)
        A = interp_monom(x)
        alpha = solve(A,b)
        residuals[i] = norm(np.dot(A,alpha) - b)
        condition[i] = cond(A)

    plt.figure()
    plt.plot(n,residuals,"-o")
    plt.grid(True)
    plt.xlabel(r"$n$")
    plt.ylabel(r"$\|A \alpha - b\|_2$")
    plt.savefig("residuals.eps")

    plt.figure()
    plt.semilogy(n,condition,"-o")
    plt.grid(True)
    plt.xlabel(r"$n$")
    plt.ylabel(r"$\log(\mathrm{cond}(A))$")
    plt.savefig("condition.eps")

    def crunch(self):
        '''Mainline to number crunch the calculation of
        member foreces.'''
# echo data:
        printline = self.printline
        lineread, noNodes, noMembers, noAuxiliaries,\
        noLoadCases, noLoadLines = self.prolog()
        nodes = self.doNodes(lineread, noNodes, noAuxiliaries)       
        printline("nodes = ")
        printdict(printline, nodes)
        members = self.doMembers(lineread, noMembers, noAuxiliaries)       
        printline('members = ') 
        printdict(printline, members)    
        memprops = self.doMemprops( noMembers, noAuxiliaries, members, nodes)
        printline('memprops =')
        printdict(self.printline, memprops)
        ndim = noNodes * 2
        printline('Dimension of connection mat ndim =' + str(ndim))
        connection = np.zeros((ndim, ndim), dtype = "float")
        for i in range(noMembers + noAuxiliaries):
            member = memprops[i + 1]
            connection[member[0] - 1, i] += member[4]
            connection[member[1] - 1, i] += member[5]
            if i < noMembers:                
                connection[member[2] - 1, i] += -member[4]            
                connection[member[3] - 1, i] += -member[5]
        printline(' ')
        printline('Connection matrix')
        printerm(printline, connection)
        try:
            conditionnumber = la.cond(connection)
            inv = la.inv(connection)
        except la.LinAlgError, e:
            printline( '%s  %s' % (type(e).__name__, e))

def check_input_data(a, b, x=None):
    """Does some sanity checks of the input data. Arguments:
        a    - input matrix
        b    - input vector
        x    - partial solution for the Jacobi Method
    Returns:
        args - list of input arguments, if valid - otherwise raises
               InputDataException
    """
    from sys import float_info
    from time import sleep
    if x is None:
        args = [a, b]
    else:
        args = [a, b, x]
    for arg in args:
        if type(arg) != np.ndarray:
            raise InputDataError(repr(np.ndarray) + ' expected, got ' +
                                 repr(type(arg)) + ' instead.')
    if (a.ndim, b.ndim, len(a)) != (2, 2, len(b)):
        raise InputDataError('Input arrays have different sizes/dimensions')
    if x is not None:
        if (x.ndim, x.size) != (2, b.size):
            raise InputDataError('Input arrays have different sizes/dimensions')
    if a.shape[0] != a.shape[1]:
        raise InputDataError('Only square matrices are supported')
    if la.cond(a) > 1 / float_info.epsilon:
        raise InputDataError('Singular matrix')
    return args

def fitmat(p, q):
    """
    Compute the best transformation to map p onto q without translation.

    The transformation is best according to the least square residuals criteria and correspond to the matrix M in:

        p-p0 = M(q-q0) + T

    where p0 and q0 are the barycentre of the list of points p and q.

    :Parameters:
        p : array(M,2)
            List of points p, one point per line, first column for x, second for y
        q : array(M,2)
            List of points q, one point per line, first column for x, second for y
    """
    pc = p.sum(0) / p.shape[0]
    qc = q.sum(0) / q.shape[0]
    p = asmatrix(p - pc)
    q = asmatrix(q - qc)
    A = p.T * p
    if cond(A) > 1e15:
        return
    V = p.T * q
    M = (A.I * V).A
    return M.T

def MdcNE(A, b):
    """
    Résolution du problème des moindres carrés linéaire :
                Min_{alpha} || b - A*alpha ||^2
    par factorisation de Cholesky du système des équations normales.

    Parameters
    ----------
    A : np.array ou np.matrix
    b : np.array ou np.matrix

    Returns
    -------
    alpha : np.array (dans tous les cas)
        solution du problème des moindres carrés linéaire
    """
    S = np.matrix(A).T * np.matrix(A)

    # Vérification au préalable du conditionnement du système et de la stabilité
    # numérique de la résolution qui va suivre
    c = la.cond(S)
    if c > 1e16:
        print('Attention : le conditionnement de la matrice des équations')
        print('            normales est très grand ---> %0.5g' % c)

    # Factorisation suivi de la résolution
    L = la.cholesky(S)           # matrice triangulaire inférieure
    m = b.size
    bvect = np.matrix( b.reshape(m,1) )
    y = A.T * bvect
    z = la.solve(L, y)
    alpha = np.array( la.solve(L.T, z) )

    return alpha

 def test_inverse(self):
     for n in xrange(1, 10):
         a = hilbert(n)
         b = invhilbert(n)
         # The Hilbert matrix is increasingly badly conditioned,
         # so take that into account in the test
         c = cond(a)
         assert_allclose(a.dot(b), eye(n), atol=1e-15*c, rtol=1e-15*c)

def const_r():
  for i in range(0, 50):
    size = 200
    mat = np.zeros((size,size))
    r   = rdft.generate_r(size)            # use constant r
    for j in range(0,size):
      for k in range(0,size):
        mat[j,k] = random.uniform(-100,100)
    f = rdft.generate_f(size)
    fr  = f.dot(r)
    fra = fr.dot(mat)
    (a_maxcond,_,_)   = rdft.get_leading_maxcond(mat)
    (fra_maxcond,_,_) = rdft.get_leading_maxcond(fra)
    a_cond   = linalg.cond(mat)
    fra_cond = linalg.cond(fra)
    print("A:  ", a_maxcond/a_cond)
    print("FRA:", fra_maxcond/fra_cond)

    def _energy(self, face, i):
        """Returns energy value and polygon area for a provided polygon."""
       
        TV1 = array([self.Mesh.vertices[face[0]], self.Mesh.vertices[face[1]], self.Mesh.vertices[face[2]]])
        TV2 = array([self.vnormal[face[0]],self.vnormal[face[1]],self.vnormal[face[2]]])
        
        if array_equal(TV1[0], TV1[1]) or array_equal(TV1[0], TV1[2]) or array_equal(TV1[1], TV1[2]):
            print "Warning: Duplicate vertices in polygon %s." % i
            print "Ignoring this polygon for energy calculation, but editing surface to remove duplicate vertices prior to DNE calculation is encouraged."
            return [0,1]

        b1 = TV1[1] - TV1[0]
        b2 = TV1[2] - TV1[0]
        
        g = array(([dot(b1,b1), dot(b1,b2)],[dot(b2,b1), dot(b2,b2)]))
        
        if self.docondition:
                if cond(g) > 10**5:
                    self.high_condition_faces.append([i, cond(g)])
                    return [0,1]
        
        c1 = TV2[1] - TV2[0]
        c2 = TV2[2] - TV2[0]
    
        fstarh = array(([dot(c1,c1), dot(c1,c2)], [dot(c2,c1), dot(c2,c2)]))
    
        gm = mat(g)  
        
        try:
            gminv = gm.I
        except LinAlgError as err:
            condition = cond(g)
            if condition > 10**5:
                err.args += ('G matrix for polygon %s is singular and an inverse cannot be determined. Condition number is %s, turning condition number checking on will cause this polygon to be ignored for energy calculation.' % (i, cond(g)),)
                raise
            else:
                err.args += ('G matrix for polygon %s is singular and an inverse cannot be determined. Condition number is %s, turning condition number checking on will not cause this polygon to be ignored for energy calculation. Further mesh processing is advised.' % (i, cond(g)),)
                raise
        
        e = trace((gminv*fstarh))
        facearea = 0.5 * sqrt(g[0,0]*g[1,1]-g[0,1]*g[1,0])
                        
        if isnan(e):
            self.nan_faces.append(i)
            
        return [e,facearea]

def diagonalize(correlator_pannel, t0, td, generalized=False):
    length = correlator_pannel.shape[0]
    n = int(np.sqrt(length))
    assert t0 is not None
    # Here we access the pannel major_xs gives time(n), mean incase it
    # was a multi correlator should have no effect on an already averaged one
    A = np.matrix(np.reshape(correlator_pannel.major_xs(td).mean().values, (n, n)))
    B = np.matrix(np.reshape(correlator_pannel.major_xs(t0).mean().values, (n, n)))
    # Require A and B to be hermition for our generalized eigen value
    # problem method to work. Here we force the matricies to be
    # hermtion. This is justified since it is just useing the other
    # measurement of the same value and averaging them.
    A = hermitionize(A)
    B = hermitionize(B)
    logging.debug("A = {} \n B = {}".format(A, B))

    if generalized:
        logging.info("running generalized eigensolver")
        evals, evecs = LA.eigh(A, b=B)  #gerenalized eig problem, eigh works only if hermitian
        evecs = np.matrix(evecs)
        V = evecs
    else:
        # Instead of using generalized eigen problem, we could solve a
        # regular eigen problem involving Binvqrt
        Binvsqrt =  LA.inv(LA.sqrtm(B))
        logging.info("condition number: {}".format(cond(Binvsqrt*A*Binvsqrt)))
        evals, evecs = LA.eigh(Binvsqrt*A*Binvsqrt)
        evecs = np.matrix(evecs)
        V = np.matrix(Binvsqrt)*evecs

    if min(evals) < 0.05:
        logging.warn("Warning, low eigenvalue detected. Eval={}".format(min(evals)))
    else:
        logging.info("lowest eigenvalue={}".format(min(evals)))
    logging.debug("eigen values are {}".format(evals))
    logging.debug("eigen vectors are {}".format(evecs))
    n = len(evecs)
    logging.debug("Matrix size {N}x{N}".format(N=n))

    def rotate(x):
        M = np.matrix(np.resize(x, (n, n)))
        M = hermitionize(M)
        D = V.H * M * V
        R = np.array(D).flatten()
        P = pd.Series(R)
        return P

    diag = correlator_pannel.apply(rotate, "items")
    diag.items = ["{}{}".format(i,j) for i in reversed(range(n)) for j in reversed(range(n))]

    # This method simultaniously diagaonlizes at t0 and td. Should be
    # identity at t0 and the eigenvalues at td
    assert compare_matrix(np.reshape(diag.major_xs(t0).mean().values, (n, n)),
                          np.identity(n)), "Rotation error: is not ~identity at t0"
    assert compare_matrix(np.reshape(diag.major_xs(td).mean().values, (n, n)),
                          np.diag(evals)), "Rotation error: Is not ~Lambda at td"

    return diag

def diagnostics(basename):
    dagfilename = join(DATADIR, basename+'.dag')
    prob = read_problem(dagfilename, plot_dag=False, show_sparsity=False)
    partial_code = prepare_evaluation_code(prob) 
    rev_ad = import_code(partial_code)
    con, jac_ad = rev_ad.evaluate(prob.refsols[0], prob.ncons, prob.nvars, prob.nzeros)
    print('Constraint infinity norm: ', norm(con, np.inf))
    print('Condition number estimate:', cond(jac_ad.todense()))
    tests.JacobianTest.dump_code(partial_code, dagfilename)

def getTransformationMat(A):
	#A = matrix([[1,5,2,7],[1,1,3,31],[1,3,4,17],[1,1,1,1]])
	#B = matrix([[77,75,74,61],[35,39,36,41],[9.2,9.2,7.2,-20.8],[1,1,1,1]])

	#A = matrix([[-7.68,-7.855,-18,-6.24],[5.46,8.912,10.35,5.3816],[110.15,121.22,124.47,129.4126],[1,1,1,1]]) #Kinect Frame
	#B = matrix([[-10,0,0,10],[25,21.22,36,21],[8.7,12.86,8.7,15],[1,1,1,1]]) #Arm Frame
	B = matrix([[-10,25,8.7,1],[0,21.22,12.86,1],[0,36,8.7,1],[10,21,15,1],[-15,31,20,1],[10,31,20,1],[10,29,15,1],[15,29,15,1],[-15,20,15,1],[-10,20,15,1],[3,18,5,1],[5,20,25,1],[-5,25,25,1],[-5,30,6,1],[15,20,6,1]]) #Arm Frame
	B = B.transpose()
	# C = (inv((A.transpose())*A))*(A.transpose())*B if pts rows
	C = B * (A.transpose()) * inv(A * (A.transpose()))
	print cond(C)
	#test = matrix([[-5.56606237302],[11.4300619608],[111.661235336],[1]]) 

	#point = C*test 
	#GoToPos(point[0],point[1],point[2],'close')

	#print (C*test)[0]
	return C

    def get_K_cond(K):
        start = time.time()

        K_cond = linalg.cond(K)

        end = time.time()
        if end - start > timer_thresh:
            print 'get_K_cond:', end - start, 'sec'
        return K_cond

def const_a(sample, size, rand_range):
  result = []
  mat = np.zeros((size,size))
  for j in range(0,size):
    for k in range(0,size):
      mat[j,k] = random.uniform(-rand_range,rand_range)
  for i in range(0, sample):
    f   = rdft.generate_f(size)
    r   = rdft.generate_r(size)
    fr  = f.dot(r)
    fra = fr.dot(mat)
    (a_maxcond,_,a_subcond)   = rdft.get_leading_maxcond(mat)
    (fra_maxcond,_,fra_subcond) = rdft.get_leading_maxcond(fra)
    a_cond   = linalg.cond(mat)
    fra_cond = linalg.cond(fra)
    result.append([mat, a_maxcond/a_cond, fra_maxcond/fra_cond, fra, a_subcond, fra_subcond])
    #print("A:  ", a_maxcond/a_cond)
    #print("FRA:", fra_maxcond/fra_cond)
  return result