def getKrivodLeft(self, order, nodes, correcFac):
        '''
        Convert Lagrange poly to normalized Legendre
        '''
        r   = sympy.Symbol('r')
        van = self.vandermonde(order, nodes)

        phi = self.lagrange(nodes)

        Np  = order + 1
        P   = sympy.zeros(1, Np)

        # Leg from Lag
        for i in range(Np):
            for j in range(Np):
                P[i] = P[i] + van.T[i, j] * phi[j] # V^T . l

        P[Np - 1] = correcFac*P[Np - 1]

        # Lag from Leg
        inv_vanT = np.linalg.inv(van.T)
        lag = sympy.zeros(1, Np)
        for i in range(Np):
            for j in range(Np):
                lag[i] = lag[i] + inv_vanT[i, j] * P[j] # (V^T)^-1 . P
                
        k_l = np.zeros(Np)
        wts = np.polynomial.legendre.leggauss(Np)[1]
        for i in range(Np):
            k_l[i] = -1*lag[i].evalf(subs = {r: -1})/wts[i]
            
        return k_l

def test_submatrix_assignment():
    m = zeros(4)
    m[2:4, 2:4] = eye(2)
    assert m == Matrix(((0,0,0,0),
                        (0,0,0,0),
                        (0,0,1,0),
                        (0,0,0,1)))
    m[0:2, 0:2] = eye(2)
    assert m == eye(4)
    m[:,0] = Matrix(4,1,(1,2,3,4))
    assert m == Matrix(((1,0,0,0),
                        (2,1,0,0),
                        (3,0,1,0),
                        (4,0,0,1)))
    m[:,:] = zeros(4)
    assert m == zeros(4)
    m[:,:] = ((1,2,3,4),(5,6,7,8),(9, 10, 11, 12),(13,14,15,16))
    assert m == Matrix(((1,2,3,4),
                        (5,6,7,8),
                        (9, 10, 11, 12),
                        (13,14,15,16)))
    m[0:2, 0] = [0,0]
    assert m == Matrix(((0,2,3,4),
                        (0,6,7,8),
                        (9, 10, 11, 12),
                        (13,14,15,16)))

 def __get_trace_facvar(self, polynomial):
     """Return dense vector representation of a polynomial. This function is
     nearly identical to __push_facvar_sparse, but instead of pushing
     sparse entries to the constraint matrices, it returns a dense
     vector.
     """
     facvar = [0] * (self.n_vars + 1)
     F = {}
     for i in range(self.matrix_var_dim):
         for j in range(self.matrix_var_dim):
             for key, value in \
                     polynomial[i, j].as_coefficients_dict().items():
                 skey = apply_substitutions(key, self.substitutions,
                                            self.pure_substitution_rules)
                 try:
                     Fk = F[skey]
                 except KeyError:
                     Fk = zeros(self.matrix_var_dim, self.matrix_var_dim)
                 Fk[i, j] += value
                 F[skey] = Fk
     # This is the tracing part
     for key, Fk in F.items():
         if key == S.One:
             k = 1
         else:
             k = self.monomial_index[key]
         for i in range(self.matrix_var_dim):
             for j in range(self.matrix_var_dim):
                 sym_matrix = zeros(self.matrix_var_dim,
                                    self.matrix_var_dim)
                 sym_matrix[i, j] = 1
                 facvar[k+i*self.matrix_var_dim+j] = (sym_matrix*Fk).trace()
     facvar = [float(f) for f in facvar]
     return facvar

    def _form_permutation_matrices(self):
        """Form the permutation matrices Pq and Pu."""

        # Extract dimension variables
        l, m, n, o, s, k = self._dims
        # Compute permutation matrices
        if n != 0:
            self._Pq = permutation_matrix(self.q, Matrix([self.q_i, self.q_d]))
            if l > 0:
                self._Pqi = self._Pq[:, :-l]
                self._Pqd = self._Pq[:, -l:]
            else:
                self._Pqi = self._Pq
                self._Pqd = Matrix()
        if o != 0:
            self._Pu = permutation_matrix(self.u, Matrix([self.u_i, self.u_d]))
            if m > 0:
                self._Pui = self._Pu[:, :-m]
                self._Pud = self._Pu[:, -m:]
            else:
                self._Pui = self._Pu
                self._Pud = Matrix()
        # Compute combination permutation matrix for computing A and B
        P_col1 = Matrix([self._Pqi, zeros(o + k, n - l)])
        P_col2 = Matrix([zeros(n, o - m), self._Pui, zeros(k, o - m)])
        if P_col1:
            if P_col2:
                self.perm_mat = P_col1.row_join(P_col2)
            else:
                self.perm_mat = P_col1
        else:
            self.perm_mat = P_col2

def interpolation(f, phi, points):
    """
    Given a function f(x), return the approximation to
    f(x) in the space V, spanned by phi, that interpolates
    f at the given points. Must have len(points) = len(phi)
    """
    N = len(phi) - 1
    A = sm.zeros((N+1, N+1))
    b = sm.zeros((N+1, 1))
    # Wrap phi and f in Python functions rather than expressions
    # so that we can evaluate phi at points[i] (alternative to subs?)
    x = sm.Symbol('x')
    phi = [sm.lambdify([x], phi[i]) for i in range(N+1)]
    f = sm.lambdify([x], f)
    print '...evaluating matrix...'
    for i in range(N+1):
        for j in range(N+1):
            print '(%d,%d)' % (i, j)
            A[i,j] = phi[j](points[i])
        b[i,0] = f(points[i])
    print
    print 'A:\n', A, '\nb:\n', b
    c = A.LUsolve(b)
    print 'coeff:', c
    u = 0
    for i in range(len(phi)):
        u += c[i,0]*phi[i](x)
    # Alternative:
    # u = sum(c[i,0]*phi[i] for i in range(len(phi)))
    print 'approximation:', u
    return u

def get_symbolic_vandermonde(p_order, x_vals=None):
    r"""Get symbolic Vandermonde matrix of evenly spaced points.

    :type p_order: int
    :param p_order: The degree of precision for the method.

    :type x_vals: list
    :param x_vals: (Optional) The list of :math:`x`-values to use. If not
                   given, defaults to ``p_order + 1`` evenly spaced points
                   on :math:`\left[0, 1\right]`.

    :rtype: tuple
    :returns: Pair of vector of powers of :math:`x` and Vandermonde matrix.
              Both are type
              :class:`sympy.Matrix <sympy.matrices.dense.MutableDenseMatrix>`,
              the ``x_vec`` is a row vector with ``p_order + 1`` columns and
              the Vandermonde matrix is square of dimension ``p_order + 1``.
    """
    x_symb = sympy.Symbol('x')
    if x_vals is None:
        x_vals = sympy.Matrix(six.moves.xrange(p_order + 1)) / p_order
    vand_mat = sympy.zeros(p_order + 1, p_order + 1)
    x_vec = sympy.zeros(1, p_order + 1)
    for i in six.moves.xrange(p_order + 1):
        x_vec[i] = x_symb**i
        for j in six.moves.xrange(p_order + 1):
            vand_mat[i, j] = x_vals[i]**j
    return x_vec, vand_mat

def regular_completion(matrix):
    m,n = matrix.shape
    r = matrix.rank()
    
    #~ IPS()
    assert m!=n, "There is no regular completion of a square matrix."

    if m<n:
        assert r==m, "Matrix does not have full row rank."
        A, B, V_pi = reshape_matrix_columns(matrix)
        zeros = sp.zeros(n-m,m)
        ones = sp.eye(n-m)
        S = st.col_stack(zeros,ones)
        completion = S*V_pi.T
        
        regular_matrix = st.row_stack(matrix,completion)
        assert st.rnd_number_rank(regular_matrix)==n, "Regular completion seems to be wrong."

    elif n<m:
        assert r==n, "Matrix does not have full column rank."
        A, B, V_pi = reshape_matrix_columns(matrix.T)
        zeros = sp.zeros(m-n,n)
        ones = sp.eye(m-n)
        S = st.col_stack(zeros,ones)
        completion = V_pi*S.T
        
        regular_matrix = st.col_stack(completion,matrix)
        assert st.rnd_number_rank(regular_matrix)==m, "Regular completion seems to be wrong."

    return completion

def rne_park_backward(rbtdef, geom, fw_results, ifunc=None):
    '''RNE backward pass.'''

    V, dV = fw_results

    if not ifunc:
        ifunc = identity

    # extend Tdh_inv so that Tdh_inv[dof] return identity
    Tdh_inv = geom.Tdh_inv + [eye(4)]

    F = list(range(rbtdef.dof + 1))
    F[rbtdef.dof] = zeros((6, 1))

    tau = zeros((rbtdef.dof, 1))

    fric = frictionforce(rbtdef)
    Idrive = driveinertiaterm(rbtdef)

    # Backward
    for i in range(rbtdef.dof - 1, -1, -1):

        Llm = (rbtdef.L[i].row_join(skew(rbtdef.l[i]))).col_join(
            (-skew(rbtdef.l[i])).row_join(eye(3) * rbtdef.m[i]))

        F[i] = Adjdual(Tdh_inv[i + 1], F[i + 1]) + \
            Llm * dV[i] - adjdual(V[i],  Llm * V[i])
        F[i] = ifunc(F[i])

        tau[i] = ifunc((geom.S[i].transpose() * F[i])[0] + fric[i] + Idrive[i])

    return tau

def get_linear_system(eqs):
    diff_eqs = eqs.substituted_expressions
    diff_eq_names = eqs.diff_eq_names
    
    symbols = [Symbol(name) for name in diff_eq_names]
    # Coefficients
    wildcards = [Wild('c_' + name, exclude=symbols) for name in diff_eq_names]
    
    #Additive constant
    constant_wildcard = Wild('c', exclude=symbols)
    
    pattern = reduce(operator.add, [c * s for c, s in zip(wildcards, symbols)])
    pattern += constant_wildcard
    
    coefficients = sp.zeros(len(diff_eq_names))
    constants = sp.zeros((len(diff_eq_names), 1))
    
    for row_idx, (name, expr) in enumerate(diff_eqs):
        s_expr = sympify(expr, locals=dict([(s.name, s) for s in symbols])).expand()
        #print s_expr.expand()
        pattern_matches = s_expr.match(pattern)
        if pattern_matches is None:
            raise ValueError(('The expression "%s", defining the variable %s, '
                             'could not be separated into linear components') %
                             (str(s_expr), name))
        
        for col_idx in xrange(len(diff_eq_names)):
            coefficients[row_idx, col_idx] = pattern_matches[wildcards[col_idx]]
        
        constants[row_idx] = pattern_matches[constant_wildcard]

    return (diff_eq_names, coefficients, constants)

 def mass_matrix_full(self):
     # Returns the mass matrix from above, augmented by kin diff's k_kqdot
     if (self._frstar is None) & (self._fr is None):
         raise ValueError("Need to compute Fr, Fr* first.")
     o = len(self._u)
     n = len(self._q)
     return ((self._k_kqdot).row_join(zeros(n, o))).col_join((zeros(o, n)).row_join(self.mass_matrix))

def rne_park_forward(rbtdef, geom, ifunc=None):
    '''RNE forward pass.'''

    if not ifunc:
        ifunc = identity

    V = list(range(0, rbtdef.dof + 1))
    dV = list(range(0, rbtdef.dof + 1))

    V[-1] = zeros((6, 1))
    dV[-1] = - zeros((3, 1)).col_join(rbtdef.gravityacc)

    # Forward
    for i in range(rbtdef.dof):

        V[i] = ifunc(Adj(geom.Tdh_inv[i], V[i - 1])) + \
            ifunc(geom.S[i] * rbtdef.dq[i])
        V[i] = ifunc(V[i])

        dV[i] = ifunc(geom.S[i] * rbtdef.ddq[i]) + \
            ifunc(Adj(geom.Tdh_inv[i], dV[i - 1])) + \
            ifunc(adj(ifunc(Adj(geom.Tdh_inv[i], V[i - 1])),
                  ifunc(geom.S[i] * rbtdef.dq[i])))
        dV[i] = ifunc(dV[i])

    return V, dV

def GetSymbolicMatrices(func,obsVariablesArray,unknownsArray,rowsForB):
    B=sp.zeros(rowsForB*len(func), len(obsVariablesArray)*rowsForB)
#     print B
    A=sp.zeros(rowsForB*len(func), len(unknownsArray))
    V=sp.zeros(len(obsVariablesArray)*rowsForB,1)
    X=sp.zeros(len(unknownsArray),1)
    #increment by size of function array (ADDING MULTIPLE FUNCTION FUNCTIONALITY)
    #loop through fun array with counter, fill in diff/wrt for each funtion while count is less than length of function array
    increment=len(func)
    vcount=0
    #range loop doesnt work so making a variable to use for indexing correct row.. i will be as if func had only 1 funtion
    row=0
    for i in Range(rowsForB):
        for j in Range(len(obsVariablesArray)):
            for level in Range(len(func)):
                B[row+level,vcount]=diff(func[level],obsVariablesArray[j])
            t=symbols('v'+obsVariablesArray[j].name+str(i+1))              #not sure if must be row as well
            V[vcount,0]=t
            vcount+=1
        row+=increment
    #             pprint (B)
    row=0   
    for i in Range(rowsForB):
        for j in Range(len(unknownsArray)):
            for level in Range(len(func)):
                A[row+level,j]=diff(func[level],unknownsArray[j])
            t=symbols('d'+unknownsArray[j].name)
            X[j,0]=t
        row+=increment
    return B,V,A,X

def test_form_2():
    symsystem2 = SymbolicSystem(
        coordinates,
        comb_implicit_rhs,
        speeds=speeds,
        mass_matrix=comb_implicit_mat,
        alg_con=alg_con_full,
        output_eqns=out_eqns,
        bodies=bodies,
        loads=loads,
    )

    assert symsystem2.coordinates == Matrix([x, y, lam])
    assert symsystem2.speeds == Matrix([u, v])
    assert symsystem2.states == Matrix([x, y, lam, u, v])

    assert symsystem2.alg_con == [4]

    inter = comb_implicit_rhs
    assert simplify(symsystem2.comb_implicit_rhs - inter) == zeros(5, 1)
    assert simplify(symsystem2.comb_implicit_mat - comb_implicit_mat) == zeros(5)

    assert set(symsystem2.dynamic_symbols()) == set([y, v, lam, u, x])
    assert type(symsystem2.dynamic_symbols()) == tuple
    assert set(symsystem2.constant_symbols()) == set([l, g, m])
    assert type(symsystem2.constant_symbols()) == tuple

    inter = comb_explicit_rhs
    symsystem2.compute_explicit_form()
    assert simplify(symsystem2.comb_explicit_rhs - inter) == zeros(5, 1)

    assert symsystem2.output_eqns == out_eqns

    assert symsystem2.bodies == (Pa,)
    assert symsystem2.loads == ((P, g * m * N.x),)

def test_n_link_pendulum_on_cart_inputs():
    l0, m0 = symbols("l0 m0")
    m1 = symbols("m1")
    g = symbols("g")
    q0, q1, F, T1 = dynamicsymbols("q0 q1 F T1")
    u0, u1 = dynamicsymbols("u0 u1")

    kane1 = models.n_link_pendulum_on_cart(1)
    massmatrix1 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
                          [-l0*m1*cos(q1), l0**2*m1]])
    forcing1 = Matrix([[-l0*m1*u1**2*sin(q1) + F], [g*l0*m1*sin(q1)]])
    assert simplify(massmatrix1 - kane1.mass_matrix) == zeros(2)
    assert simplify(forcing1 - kane1.forcing) == Matrix([0, 0])

    kane2 = models.n_link_pendulum_on_cart(1, False)
    massmatrix2 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
                          [-l0*m1*cos(q1), l0**2*m1]])
    forcing2 = Matrix([[-l0*m1*u1**2*sin(q1)], [g*l0*m1*sin(q1)]])
    assert simplify(massmatrix2 - kane2.mass_matrix) == zeros(2)
    assert simplify(forcing2 - kane2.forcing) == Matrix([0, 0])

    kane3 = models.n_link_pendulum_on_cart(1, False, True)
    massmatrix3 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
                          [-l0*m1*cos(q1), l0**2*m1]])
    forcing3 = Matrix([[-l0*m1*u1**2*sin(q1)], [g*l0*m1*sin(q1) + T1]])
    assert simplify(massmatrix3 - kane3.mass_matrix) == zeros(2)
    assert simplify(forcing3 - kane3.forcing) == Matrix([0, 0])

    kane4 = models.n_link_pendulum_on_cart(1, True, False)
    massmatrix4 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
                          [-l0*m1*cos(q1), l0**2*m1]])
    forcing4 = Matrix([[-l0*m1*u1**2*sin(q1) + F], [g*l0*m1*sin(q1)]])
    assert simplify(massmatrix4 - kane4.mass_matrix) == zeros(2)
    assert simplify(forcing4 - kane4.forcing) == Matrix([0, 0])

def least_squares_non_verbose(f, psi, Omega, symbolic=True):
    """
    Given a function f(x) on an interval Omega (2-list)
    return the best approximation to f(x) in the space V
    spanned by the functions in the list psi.
    """
    N = len(psi) - 1
    A = sym.zeros((N+1, N+1))
    b = sym.zeros((N+1, 1))
    x = sym.Symbol('x')
    for i in range(N+1):
        for j in range(i, N+1):
            integrand = psi[i]*psi[j]
            integrand = sym.lambdify([x], integrand)
            I = sym.mpmath.quad(integrand, [Omega[0], Omega[1]])
            A[i,j] = A[j,i] = I
        integrand = psi[i]*f
        integrand = sym.lambdify([x], integrand)
        I = sym.mpmath.quad(integrand, [Omega[0], Omega[1]])
        b[i,0] = I
    c = sym.mpmath.lu_solve(A, b)  # numerical solve
    c = [c[i,0] for i in range(c.rows)]

    u = sum(c[i]*psi[i] for i in range(len(psi)))
    return u, c

    def _form_coefficient_matrices(self):
        """Form the coefficient matrices C_0, C_1, and C_2."""

        # Extract dimension variables
        l, m, n, o, s, k = self._dims
        # Build up the coefficient matrices C_0, C_1, and C_2
        # If there are configuration constraints (l > 0), form C_0 as normal.
        # If not, C_0 is I_(nxn). Note that this works even if n=0
        if l > 0:
            f_c_jac_q = self.f_c.jacobian(self.q)
            self._C_0 = (eye(n) - self._Pqd * (f_c_jac_q *
                    self._Pqd).LUsolve(f_c_jac_q)) * self._Pqi
        else:
            self._C_0 = eye(n)
        # If there are motion constraints (m > 0), form C_1 and C_2 as normal.
        # If not, C_1 is 0, and C_2 is I_(oxo). Note that this works even if
        # o = 0.
        if m > 0:
            f_v_jac_u = self.f_v.jacobian(self.u)
            temp = f_v_jac_u * self._Pud
            if n != 0:
                f_v_jac_q = self.f_v.jacobian(self.q)
                self._C_1 = -self._Pud * temp.LUsolve(f_v_jac_q)
            else:
                self._C_1 = zeros(o, n)
            self._C_2 = (eye(o) - self._Pud *
                    temp.LUsolve(f_v_jac_u)) * self._Pui
        else:
            self._C_1 = zeros(o, n)
            self._C_2 = eye(o)

def interpolation(f, psi, points):
    """
    Given a function f(x), return the approximation to
    f(x) in the space V, spanned by psi, that interpolates
    f at the given points. Must have len(points) = len(psi)
    """
    N = len(psi) - 1
    A = sym.zeros((N+1, N+1))
    b = sym.zeros((N+1, 1))
    # Wrap psi and f in Python functions rather than expressions
    # so that we can evaluate psi at points[i] (alternative to subs?)
    psi_sym = psi # save symbolic expression
    x = sym.Symbol('x')
    psi = [sym.lambdify([x], psi[i]) for i in range(N+1)]
    f = sym.lambdify([x], f)
    print '...evaluating matrix...'
    for i in range(N+1):
        for j in range(N+1):
            print '(%d,%d)' % (i, j)
            A[i,j] = psi[j](points[i])
        b[i,0] = f(points[i])
    print
    print 'A:\n', A, '\nb:\n', b
    c = A.LUsolve(b)
    # c is a sympy Matrix object, turn to list
    c = [sym.simplify(c[i,0]) for i in range(c.shape[0])]
    print 'coeff:', c
#    u = sym.simplify(sum(c[i,0]*psi_sym[i] for i in range(N+1)))
    u = sym.simplify(sum(c[i]*psi_sym[i] for i in range(N+1)))
    print 'approximation:', u
    return u, c

def gen_XdX(atom_list,operator_table,operator_table_dagger,Hcomm,N_atoms_uc):
    """Operate commutation relations to put all the 2nd order term as ckd**ck, cmk**cmkd, cmk**ck and ckd**cmkd form"""
    print "gen_XdX"
    exclude_list=[]
    coeff_list=[]

    print N_atoms_uc
    N_int = max([max(atom_list[i].neighbors) for i in range(N_atoms_uc)])+1

    XdX=sympy.zeros(2*N_atoms_uc)
    g=sympy.zeros(2*N_atoms_uc)
    for i in range(2*N_atoms_uc):
        curr_row=[]
        for j in range(2*N_atoms_uc):
            mycoeff=operator_table_dagger[i]*operator_table[j]
            exclude_list.append(mycoeff)
            currcoeff1=coeff(Hcomm[i,j].expand(),mycoeff)
            currcoeff2=coeff(Hcomm[j,i].expand(),mycoeff)
            print Hcomm[i,j]
            print mycoeff
            print currcoeff1,currcoeff2
            if currcoeff1!=None and currcoeff2!=None:
                XdX[i,j]=currcoeff1+currcoeff2
                curr_row.append(currcoeff1+currcoeff2)
            if i!=j:
                g[i,j]=0
            else:
                if i>=N_atoms_uc:
                    g[i,j]=-1
                else:
                    g[i,j]=1
    print 'XdX done'
    return XdX,g

    def _computeTransitionMeanVar(self):
        '''
        This is the mean and variance information that we need
        for the :math:`\tau`-Leap
        '''
    
        if self._transitionJacobian is None or self._hasNewTransition:
            self._computeTransitionJacobian()

        F = self._transitionJacobian
        # holders
        mu = sympy.zeros(self._numTransition, 1)
        sigma2 = sympy.zeros(self._numTransition, 1)
        # we calculate the mean and variance
        for i in range(self._numTransition):
            for j, eqn in enumerate(self._transitionVector):
                mu[i] += F[i,j] * eqn
                sigma2[i] += F[i,j] * F[i,j] * eqn

        self._transitionMean = mu
        self._transitionVar = sigma2

        # now we are going to compile them
        if self._isDifficult:
            self._transitionMeanCompile = self._SC.compileExprAndFormat(self._sp,
                                                                        self._transitionMean,
                                                                        modules=modulesH)
            self._transitionVarCompile = self._SC.compileExprAndFormat(self._sp,
                                                                       self._transitionVar,
                                                                       modules=modulesH)
        else:
            self._transitionMeanCompile = self._SC.compileExprAndFormat(self._sp, self._transitionMean)
            self._transitionVarCompile = self._SC.compileExprAndFormat(self._sp, self._transitionVar)

        return None

def met_zeyd(A, b):
    C, d = iteration_view(A, b)
    H = sympy.zeros(3)
    F = sympy.zeros(3)
    c = sympy.zeros(3,1)
    for i in xrange(3):
        c[i] = d[i]
        for j in xrange(3):
            if i > j:
                H[i, j] = C[i, j]
            else:
                F[i, j] = C[i, j]
    print "\nx = Cx + d\n"
    print "C = \n", C, "\n", "\nd = \n", d
    print "\nConvergence: ", convergence_mzeyd(C, d)
    if convergence_mzeyd(C, d):
        E = sympy.eye(3)
        x0 = sympy.ones(3, 1)
        x1 = (E-H).inv()*F*x0 + (E-H).inv()*c
        while ((x1-x0)[0] > 0.00001 or (x1-x0)[1] > 0.00001 or\
              (x1-x0)[2] > 0.00001 or (x0-x1)[0] > 0.00001 or\
              (x0-x1)[1] > 0.00001 or (x0-x1)[2] > 0.00001):
              x0 = x1
              x1 = (E-H).inv()*F*x0 + (E-H).inv()*c
        print "\nSolution:" 

    return [element for element in x1]