def get_poly(order, dim, is_simplex=False):
    """
    Construct a polynomial of given `order` in space dimension `dim`,
    and integrate it symbolically over a rectangular or simplex domain
    for coordinates in [0, 1].
    """
    try:
        xs = sm.symarray('x', dim)
    except:
        xs = sm.symarray(dim, 'x')

    opd = max(1, int((order + 1) / dim))

    poly = 1.0
    oo = 0
    for x in xs:
        if (oo + opd) > order:
            opd = order - oo
            
        poly *= (x**opd + 1)
        oo += opd

    limits = [[xs[ii], 0, 1] for ii in range(dim)]
    if is_simplex:
        for ii in range(1, dim):
            for ip in range(0, ii):
                limits[ii][2] -= xs[ip]

    integral = sm.integrate(poly, *reversed(limits))

    return xs, poly, limits, integral

def main(y0='1,0', mu=1.0, tend=10., nt=50, savefig='None', plot=False,
         savetxt='None', integrator='scipy', dpi=100, kwargs='',
         verbose=False):
    assert nt > 1
    y = sp.symarray('y', 2)
    p = sp.Symbol('p', real=True)
    f = [y[1], -y[0] + p*y[1]*(1 - y[0]**2)]
    odesys = SymbolicSys(zip(y, f), params=[p], names=True)
    tout = np.linspace(0, tend, nt)
    y0 = list(map(float, y0.split(',')))
    kwargs = dict(eval(kwargs) if kwargs else {})
    xout, yout, info = odesys.integrate(
        tout, y0, [mu], integrator=integrator, **kwargs)
    if verbose:
        print(info)
    if savetxt != 'None':
        np.savetxt(stack_1d_on_left(xout, yout), savetxt)
    if plot:
        import matplotlib.pyplot as plt
        odesys.plot_result()
        plt.legend()
        if savefig != 'None':
            plt.savefig(savefig, dpi=dpi)
        else:
            plt.show()

def main(init_conc='1e-7,1e-7,1e-7,1,55.5',
         lnKa=-21.28, lnKw=-36.25,
         savetxt='None', verbose=False,
         rref=False, charge=False, solver='scipy'):
    # H+, OH- NH4+, NH3, H2O
    iHp, iOHm, iNH4p, iNH3, iH2O = init_conc = map(float, init_conc.split(','))
    lHp, lOHm, lNH4p, lNH3, lH2O = x = sp.symarray('x', 5)
    Hp, OHm, NH4p, NH3, H2O = map(sp.exp, x)
    # lHp + lOHm - lH2O - lnKw,
    # lHp + lNH3 - lNH4p - lnKa,
    coeffs = [[1, 1, 0, 0, -1], [1, 0, -1, 1, 0]]
    vals = [lnKw, lnKa]
    lp = linear_exprs(coeffs, x, vals, rref=rref)
    f = lp + [
        Hp + OHm + 4*NH4p + 3*NH3 + 2*H2O - (
            iHp + iOHm + 4*iNH4p + 3*iNH3 + 2*iH2O),  # H
        NH4p + NH3 - (iNH4p + iNH3),  # N
        OHm + H2O - (iOHm + iH2O)
    ]
    if charge:
        f += [Hp - OHm + NH4p - (iHp - iOHm + iNH4p)]

    neqsys = SymbolicSys(x, f)
    x, sol = neqsys.solve([0]*5, solver=solver)
    if verbose:
        print(np.exp(x), sol)
    else:
        print(np.exp(x))
    assert sol.success

    def initialize(self, args):
        self.E   = args[0]
        self.A   = args[1]
        self.I   = args[2]
        self.r   = args[3]
        self.rho = args[4]
        self.l   = args[5]
        self.g   = args[6]

        q = sym.Matrix(sym.symarray('q',6))
        E, A, I, r, rho, l, g = sym.symbols('E A I r rho l g')
        theta = sym.Matrix(['theta_1','theta_2'])
        omega = sym.Matrix(['omega_1','omega_2'])
        
        # Load symbolic needed matricies and vectors
        M_sym      = pickle.load( open( "gebf-mass-matrix.dump",  "rb" ) )
        beta_sym   = pickle.load( open( "gebf-force-vector.dump", "rb" ) )
        Gamma1_sym = pickle.load( open( "gebf-1c-matrix.dump",    "rb" ) )
        Gamma2_sym = pickle.load( open( "gebf-2c-matrix.dump",    "rb" ) )

        # Create numeric function of needed matrix and vector quantities
        # this is MUCH MUCH faster than .subs()
        # .subs() is unusably slow !!
        self.M      = lambdify((E, A, I, r, rho, l, g, q, theta, omega),      M_sym, "numpy")
        self.beta   = lambdify((E, A, I, r, rho, l, g, q, theta, omega),   beta_sym, "numpy")
        self.Gamma1 = lambdify((E, A, I, r, rho, l, g, q, theta, omega), Gamma1_sym, "numpy")
        self.Gamma2 = lambdify((E, A, I, r, rho, l, g, q, theta, omega), Gamma2_sym, "numpy")

def construct_matrix_and_entries(prefix_name,shape):
    
    A_entries = matrix(sympy.symarray(prefix_name,shape))
    A  = sympy.Matrix.zeros(shape[0],shape[1])
    for r in range(A.rows):
        for c in range(A.cols):
            A[r,c] = A_entries[r,c]
    return A, A_entries

    def intProps(self,args):
        
        u = args[0]

        # symbolic system parameters 
        E, A, I, r, rho, l, = sym.symbols('E A, I r rho l')

        # Load symbolic mass matrix and substitute material properties
        self.M = pickle.load( open( "gebf-mass-matrix.dump", "rb" ) ).subs([ \
            (E, self.E), (A, self.A), (I, self.I), (r, self.r), (rho, self.rho), (l, self.l)])
        
        # load the body and applied force vector and substitute material properties
        self.beta = pickle.load( open( "gebf-beta.dump", "rb" ) ).subs([ \
            (E, self.E), (A, self.A), (I, self.I), (r, self.r), (rho, self.rho), (l, self.l)])
        
        # Substitute State Variables
        # re-make symbols for proper substitution and create the paird list
        q = sym.Matrix(sym.symarray('q',2*3))
        qe = [self.theta1] + u[:2] + [self.theta2] + u[2:]        
        q_sub = [(q, qi) for q, qi in zip(q, qe)]
       
        # Substitute state variables
        self.beta = self.beta.subs(q_sub).evalf()
        self.M = self.M.subs(q_sub).evalf()

        # Form the binary-DCA algebraic quantities
        # Partition mass matrix
        self.M11 = np.array(self.M[0:3,0:3])
        self.M12 = np.array(self.M[0:3,3:6])
        self.M21 = np.array(self.M[3:6,0:3])
        self.M22 = np.array(self.M[3:6,3:6])

        # For now use these definitions to cast Fic (constraint forces between GEBF elements) 
        # into generalized constraint forces
        self.gamma11 = np.eye(3)
        self.gamma12 = np.zeros((3,3))
        self.gamma22 = np.eye(3)
        self.gamma21 = np.zeros((3,3))
        
        # partition beta into lambda13 and lambda23
        self.gamma13 = np.array(self.beta[0:3])
        self.gamma23 = np.array(self.beta[3:6])


        # Commonly inverted quantities
        self.iM11 = inv(self.M11)
        self.iM22 = inv(self.M22)
        self.Gamma1 = inv(self.M11 - self.M12*self.iM22*self.M21)
        self.Gamma2 = inv(self.M22 - self.M21*self.iM11*self.M12)

        # Compute all terms of the two handle equations
        self.z11 = self.Gamma1.dot(self.gamma11 - self.M12.dot(self.iM22.dot(self.gamma21)))
        self.z12 = self.Gamma1.dot(self.gamma12 - self.M12.dot(self.iM22.dot(self.gamma22)))
        self.z13 = self.Gamma1.dot(self.gamma13 - self.M12.dot(self.iM22.dot(self.gamma23)))

        self.z21 = self.Gamma2.dot(self.gamma21 - self.M21.dot(self.iM11.dot(self.gamma11)))
        self.z22 = self.Gamma2.dot(self.gamma22 - self.M21.dot(self.iM11.dot(self.gamma12)))
        self.z23 = self.Gamma2.dot(self.gamma23 - self.M21.dot(self.iM11.dot(self.gamma13)))

    def __init__(self, modulus, inertia, radius, density, length, state):
        """
        Initializes all of the inertial propertias of the element and assigns 
        values to physical and material properties 
        """
        # symbolic system parameters 
        E, I, r, rho, l, = sym.symbols('E I r rho l')

        # Load symbolic mass matrix and substitute material properties
        M = pickle.load( open( "gebf-mass-matrix.dump", "rb" ) ).subs([ \
            (E, modulus), (I, inertia), (r, radius), (rho, density), (l, length)])
        
        # load the body and applied force vector and substitute material properties
        self.beta = pickle.load( open( "gebf-beta.dump", "rb" ) ).subs([ \
            (E, modulus), (I, inertia), (r, radius), (rho, density), (l, length)])
        
        # Substitute State Variables
        # re-make symbols for proper substitution and create the paird list
        q = sym.Matrix(sym.symarray('q',2*8))
        q_sub = [(q, qi) for q, qi in zip(q, state)]
       
        # Substitute state variables
        beta = self.beta.subs(q_sub)
        M = M.subs(q_sub)

        # Form the binary-DCA algebraic quantities
        # Partition mass matrix
        M11 = np.array(M[0:4,0:4])
        M12 = np.array(M[0:4,4:8])
        M21 = np.array(M[4:8,0:4])
        M22 = np.array(M[4:8,4:8])

        # For now use these definitions to cast Fic (constraint forces between GEBF elements) 
        # into generalized constraint forces
        self.gamma11 = np.eye(4)
        self.gamma12 = np.zeros((4,4))
        self.gamma22 = np.eye(4)
        self.gamma21 = np.zeros((4,4))
        
        # partition beta into lambda13 and lambda23
        self.gamma13 = np.array(beta[0:4])
        self.gamma23 = np.array(beta[4:8])


        # Commonly inverted quantities
        iM11 = inv(self.M11)
        iM22 = inv(self.M22)
        Gamma1 = inv(self.M11 - self.M12*iM22*self.M21)
        Gamma2 = inv(self.M22 - self.M21*iM11*self.M12)

        # Compute all terms of the two handle equations
        self.z11 = Gamma1.dot(self.gamma11 - self.M12.dot(iM22.dot(self.gamma21)))
        self.z12 = Gamma1.dot(self.gamma12 - self.M12.dot(iM22.dot(self.gamma22)))
        self.z13 = Gamma1.dot(self.gamma13 - self.M12.dot(iM22.dot(self.gamma23)))

        self.z21 = Gamma2.dot(self.gamma21 - self.M21.dot(iM11.dot(self.gamma11)))
        self.z22 = Gamma2.dot(self.gamma22 - self.M21.dot(iM11.dot(self.gamma12)))
        self.z23 = Gamma2.dot(self.gamma23 - self.M21.dot(iM11.dot(self.gamma13)))

def test_SymbolicSys_bateman(band):
    tend, k, y0 = 2, [4, 3], (5, 4, 2)
    y = sp.symarray('y', len(k)+1)
    dydt = decay_dydt_factory(k)
    f = dydt(0, y)
    odesys = SymbolicSys(zip(y, f), band=band)
    xout, yout, info = odesys.integrate(tend, y0, integrator='scipy')
    ref = np.array(bateman_full(y0, k+[0], xout-xout[0], exp=np.exp)).T
    assert np.allclose(yout, ref)

def test_symarray():
    """Test creation of numpy arrays of sympy symbols."""

    import numpy as np
    import numpy.testing as npt
    
    syms = symbols('_0 _1 _2')
    s1 = symarray(3)
    s2 = symarray(3)
    npt.assert_array_equal (s1, np.array(syms, dtype=object))
    assert s1[0] is s2[0]
    
    a = symarray(3, 'a')
    b = symarray(3, 'b')
    assert not(a[0] is b[0])

    asyms = symbols('a_0 a_1 a_2')
    npt.assert_array_equal (a, np.array(asyms, dtype=object))

    # Multidimensional checks
    a2d = symarray((2,3), 'a')
    assert a2d.shape == (2,3)
    a00, a12 = symbols('a_0_0, a_1_2')
    assert a2d[0,0] is a00
    assert a2d[1,2] is a12

    a3d = symarray((2,3,2), 'a')
    assert a3d.shape == (2,3,2)
    a000, a120, a121 = symbols('a_0_0_0, a_1_2_0 a_1_2_1')
    assert a3d[0,0,0] is a000
    assert a3d[1,2,0] is a120
    assert a3d[1,2,1] is a121

def test_linear_exprs():
    a, b, c = x = sp.symarray('x', 3)
    coeffs = [[1, 3, -2],
              [3, 5, 6],
              [2, 4, 3]]
    vals = [5, 7, 8]
    exprs = linear_exprs(coeffs, x, vals)
    known = [1*a + 3*b - 2*c - 5,
             3*a + 5*b + 6*c - 7,
             2*a + 4*b + 3*c - 8]
    assert all([(rt - kn).simplify() == 0 for rt, kn in zip(exprs, known)])

    rexprs = linear_exprs(coeffs, x, vals, rref=True)
    rknown = [a + 15, b - 8, c - 2]
    assert all([(rt - kn).simplify() == 0 for rt, kn in zip(rexprs, rknown)])

    def __init__(self,name,func,argnames=None):
        """
        Args:
            name (str): the symbolic module name of the function.
            func (sympy.Function): the Sympy function
            argnames (list of strs, optional): provided if you don't
                want to use 'x','y', 'z' for the argument names.
        """
        assert isinstance(func,sympy.FunctionClass)
        if hasattr(func,'nargs'):
            if len(func.nargs) > 1:
                print("SympyFunctionAdaptor: can't yet handle multi-argument functions")
            nargs = None
            for i in func.nargs:
                nargs = i
        else:
            nargs = 1
        if argnames is None:
            if nargs == 1:
                argnames = ['x']
            elif nargs <= 3:
                argnames = [['x','y','z'][i] for i in len(nargs)]
            else:
                argnames = ['arg'+str(i+1) for i in len(nargs)]
        Function.__init__(self,name,func,argnames)

        self.deriv = [None]*len(argnames)
        self.jacobian = [None]*len(argnames)
        self.sympy_jacobian = [None]*len(argnames)
        self.sympy_jacobian_funcs = [None]*len(argnames)

        xs = sympy.symarray('x',nargs)
        for i,arg in enumerate(argnames):
            self.sympy_jacobian[i] = func(*xs).diff(xs[i])
        
        for i in range(len(argnames)):
            def cache_jacobian(*args):
                if self.sympy_jacobian_funcs[i] is None:
                    #print "Creating jacobian function",name + "_jac_" + argnames[i]
                    self.sympy_jacobian_funcs[i] = SympyFunction(name + "_jac_" + argnames[i], self.sympy_jacobian[i],argnames)
                return OperatorExpression(self.sympy_jacobian_funcs[i],args)
            self.jacobian[i] = cache_jacobian

	def generate_jac_lambda(self):
		"""
		translates the symbolic Jacobian to a function using SymPy’s `lambdify <http://docs.sympy.org/latest/modules/utilities/lambdify.html>`_ tool. If the symbolic Jacobian has not been generated, it is generated by calling `generate_jac_sym`.
		"""
		
		if self.helpers:
			warn("Lambdification handles helpers by pluggin them in. This may be very ineficient")
		
		self._generate_jac_sym()
		
		jac_matrix = sympy.Matrix([ [entry for entry in line] for line in self.jac_sym ])
		
		t,y = provide_basic_symbols()
		Y = sympy.symarray("Y", self.n)
		
		substitutions = self.helpers[::-1] + [(y(i),Y[i]) for i in range(self.n)]
		jac_subsed = jac_matrix.subs(substitutions)
		JAC = sympy.lambdify([t]+[Yentry for Yentry in Y], jac_subsed)
		
		self.jac = lambda t,ypsilon: array(JAC(t,*ypsilon))

def differentials(f,x,y):
    """
    Returns a basis of the holomorphic differentials defined on the
    Riemann surface `X: f(x,y) = 0`.

    Input:

    - f: a Sympy object describing a complex plane algebraic curve.
    
    - x,y: the independent and dependent variables, respectively.
    """
    d = f.as_poly().total_degree()
    n = sympy.degree(f,y)

    # coeffiecients and general adjoint polynomial
    c_arr = sympy.symarray('c',(d-3,d-3)).tolist()
    c = dict( (cij,(c_arr.index(ci),ci.index(cij))) 
              for ci in c_arr for cij in ci )
    P = sum( c_arr[i][j]*x**i*y**j 
             for i in range(d-3) for j in range(d-3) if i+j <= d-3)

    S = singularities(f,x,y)
    differentials = set([])
    for (alpha, beta, gamma), (m,delta,r) in S:
        g,u,v,u0,v0 = _transform(f,x,y,(alpha,beta,gamma))

#        if delta > m*(m-1)/2:
        if True:
            # Use integral basis method.
            b = integral_basis(g,u,v)
            for bi in b:
                monoms = _adjoint_monomials(f,x,y,bi,P,c)
                differentials.add(monom for monom in monoms) 
        else:
            # Use Puiseux series method
            b = integral_basis(g,u,v)


    return [differential/sympy.diff(f,y) for differential in differentials]

	def generate_f_lambda(self, simplify=True):
		"""
		translates the symbolic derivative to a function using SymPy’s `lambdify <http://docs.sympy.org/latest/modules/utilities/lambdify.html>`_ tool.
		
		Parameters
		----------
		simplify : boolean
			Whether the derivative should be `simplified <http://docs.sympy.org/dev/modules/simplify/simplify.html>`_ (with `ratio=1.0`) before translating to C code. The main reason why you could want to disable this is if your derivative is already optimised and so large that simplifying takes a considerable amount of time.
		"""
		
		if self.helpers:
			warn("Lambdification does not handle helpers in an efficient manner.")
		
		t,y = provide_basic_symbols()
		Y = sympy.symarray("Y", self.n)
		
		substitutions = self.helpers[::-1] + [(y(i),Y[i]) for i in range(self.n)]
		f_sym_wc = (entry.subs(substitutions) for entry in f_sym)
		if simplify:
			f_sym_wc = (entry.simplify(ratio=1.0) for entry in f_sym)
		F = sympy.lambdify([t]+[Yentry for Yentry in Y], list(f_sym_wc))
		
		self.f = lambda t,ypsilon: array(F(t,*ypsilon)).flatten()

from time import clock
import numpy as np
import sympy as sp
import symengine as se

# Real-life example (ion speciation problem in water chemistry)

x = sp.symarray('x', 14)
p = sp.symarray('p', 14)
args = np.concatenate((x, p))
exp = sp.exp
exprs = [x[0] + x[1] - x[4] + 36.252574322669, x[0] - x[2] + x[3] + 21.3219379611249, x[3] + x[5] - x[6] + 9.9011158998744, 2*x[3] + x[5] - x[7] + 18.190422234653, 3*x[3] + x[5] - x[8] + 24.8679190043357, 4*x[3] + x[5] - x[9] + 29.9336062089226, -x[10] + 5*x[3] + x[5] + 28.5520551531262, 2*x[0] + x[11] - 2*x[4] - 2*x[5] + 32.4401680272417, 3*x[1] - x[12] + x[5] + 34.9992934135095, 4*x[1] - x[13] + x[5] + 37.0716199972041, p[0] - p[1] + 2*p[10] + 2*p[11] - p[12] - 2*p[13] + p[2] + 2*p[5] + 2*p[6] + 2*p[7] + 2*p[8] + 2*p[9] - exp(x[0]) + exp(x[1]) - 2*exp(x[10]) - 2*exp(x[11]) + exp(x[12]) + 2*exp(x[13]) - exp(x[2]) - 2*exp(x[5]) - 2*exp(x[6]) - 2*exp(x[7]) - 2*exp(x[8]) - 2*exp(x[9]), -p[0] - p[1] - 15*p[10] - 2*p[11] - 3*p[12] - 4*p[13] - 4*p[2] - 3*p[3] - 2*p[4] - 3*p[6] - 6*p[7] - 9*p[8] - 12*p[9] + exp(x[0]) + exp(x[1]) + 15*exp(x[10]) + 2*exp(x[11]) + 3*exp(x[12]) + 4*exp(x[13]) + 4*exp(x[2]) + 3*exp(x[3]) + 2*exp(x[4]) + 3*exp(x[6]) + 6*exp(x[7]) + 9*exp(x[8]) + 12*exp(x[9]), -5*p[10] - p[2] - p[3] - p[6] - 2*p[7] - 3*p[8] - 4*p[9] + 5*exp(x[10]) + exp(x[2]) + exp(x[3]) + exp(x[6]) + 2*exp(x[7]) + 3*exp(x[8]) + 4*exp(x[9]), -p[1] - 2*p[11] - 3*p[12] - 4*p[13] - p[4] + exp(x[1]) + 2*exp(x[11]) + 3*exp(x[12]) + 4*exp(x[13]) + exp(x[4]), -p[10] - 2*p[11] - p[12] - p[13] - p[5] - p[6] - p[7] - p[8] - p[9] + exp(x[10]) + 2*exp(x[11]) + exp(x[12]) + exp(x[13]) + exp(x[5]) + exp(x[6]) + exp(x[7]) + exp(x[8]) + exp(x[9])]
lmb_symengine = se.Lambdify(args, exprs)
lmb_sympy = sp.lambdify(args, exprs)

inp = np.ones(28)

tim_symengine = clock()
res_symengine = np.empty(len(exprs))
for i in range(500):
    res_symengine = lmb_symengine(inp)
    #lmb_symengine.unsafe_real_real(inp, res_symengine)
tim_symengine = clock() - tim_symengine

tim_sympy = clock()
for i in range(500):
    res_sympy = lmb_sympy(*inp)
tim_sympy = clock() - tim_sympy

print('symengine speed-up factor (higher is better) vs sympy: %12.5g' %
      (tim_sympy/tim_symengine))

def exprToSympy(expr):
    if isinstance(expr,Variable):
        if expr.type.is_scalar():
            return sympy.Symbol(expr.name)
        elif expr.type.type in ARRAY_TYPES: 
            #1D column vector
            assert expr.type.size is not None,"Can't convert variable-sized arrays to Sympy Matrix's"
            entries = sympy.symarray(expr.name,expr.type.size)
            return sympy.Matrix(entries)
        else: 
            raise ValueError("Invalid Variable")
    elif isinstance(expr,VariableExpression):
        return exprToSympy(expr.var)
    elif isinstance(expr,UserDataExpression):
        return sympy.Symbol(expr.name)
    elif isinstance(expr,ConstantExpression):
        return exprToSympy(expr.value)
    elif isinstance(expr,OperatorExpression):
        fname = expr.functionInfo.name
        sname = fname.capitalize()
        sargs = [exprToSympy(a) for a in expr.args]
        if fname in _sympySpecialConstructors:
            return _sympySpecialConstructors[fname](expr.args,sargs)
        if fname in _sympyOperators:
            try:
                return getattr(operator,fname)(*sargs)
            except Exception as e:
                print("exprToSympy: Error raised while performing operator %s on arguments %s"%(fname,str(expr)))
                raise
        if hasattr(sympy,sname):
            #try capitalized version first
            try:
                return getattr(sympy,sname)(*sargs)
            except Exception:
                print("exprToSympy: Error raised while trying sympy.%s on arguments %s"%(sname,str(expr)))
        if hasattr(sympy,fname):
            #numpy equivalents, like eye
            try:
                return getattr(sympy,fname)(*sargs)
            except Exception:
                print("exprToSympy: Error raised while trying sympy.%s on arguments %s"%(fname,str(expr)))
        if isinstance(expr.functionInfo.func, collections.Callable):
            print("exprToSympy: Function %s does not have Sympy equivalent, returning adaptor "%(fname,))
            return _make_sympy_adaptor(expr.functionInfo)(*sargs)
        else:
            print("exprToSympy: Function %s does not have Sympy equivalent, expanding expression"%(fname,))
            assert isinstance(expr.functionInfo.func,Expression)
            sfunc = exprToSympy(expr.functionInfo.func)
            return sfunc.subs(list(zip(expr.functionInfo.argNames,sargs)))
        print("exprToSympy: Function %s does not have Sympy equivalent, returning generic Function"%(fname,))
        return sympy.Function(fname)(*sargs)
    else:
        if hasattr(expr,'__iter__'):
            if hasattr(expr[0],'__iter__'):
                #Matrix
                assert not hasattr(expr[0][0],'__iter__'),"Sympy can't handle tensors yet"
                return sympy.Matrix(expr)
            else:
                #1-D vector -- treat as column vector
                return sympy.Matrix(expr)
        if isinstance(expr,(float,int,bool)):
            return sympify(expr)

v = sym.simplify(udot + R*Omega_skew*s)
print('v = ')
display(v)

# Define acceleration of element endpoints (nodes)
a = sym.simplify(uddot + R*Omega_skew*Omega_skew*s + R*Alpha_skew*s)
print('\na = ')
display(a)


# ### Compute the Mass Matrix

# In[ ]:

# Define shape function for element with one node at each end
h = sym.symarray('h', 2)

h[0] = sym.Rational(1,2)*(1 - x)
h[1] = sym.Rational(1,2)*(1 + x)

# Compute shape function matrix
H = sym.expand(sym.Matrix([h[0]*sym.eye(3), h[1]*sym.eye(3)])).T
print('\nH = ')
display(H.T)

# Define velocity of any point 
Vp = H*v
print('\nV = ')
display(Vp)

# Define velocity of any point 

# Kinematic values of previos nodes (generic)
# e.g., omega_node  = omega + qdot
theta = sym.Matrix(['theta_1','theta_2'])
omega = sym.Matrix(['omega_1','omega_2'])
alpha = sym.Matrix(['alpha_1','alpha_2'])

# coordinates of the point in the 2D cross-section
# of nodes one and two 
s1 = sym.Matrix(['r_2','r_3'])
s2 = sym.Matrix(['r_2','r_3'])
s = sym.Matrix.vstack(s1,s2)

# generalized coordinates
# one rotation and two displacements per-node (two nodes per element)
# in this version generalzied speeds are qdots
q = sym.Matrix(sym.symarray('q',6))
qdot = sym.Matrix(sym.symarray('qdot',len(q)))
qddot = sym.Matrix(sym.symarray('qddot',len(q)))

# Deformations of Nodes (u's are not generalized speeds) 
u = sym.Matrix([q[1:3,0], q[4:6,0]])
udot = sym.Matrix([qdot[1:3,0], qdot[4:8,0]])
uddot = sym.Matrix([qddot[1:3,0], qddot[4:6,0]])

# ### Needed Matrix Quantities
""" 
Some cheating here: 
q0,q3 and q0dot,q3dot are really theta_1j, theta_2j and omega_1j, omega_2j
"""
# angular position and velocity for 2D using relative coordinates
# the sum of the respective quantites of bodies 1 - bodyj-1 

 def __init__(self, a=0.0, b=1.0, n=5, bv={},
              tag='', use_std_approach=False, **kwargs):
     # there are two different approaches implemented for evaluating
     # the splines which mainly differ in the node that is used in the 
     # evaluation of the polynomial parts
     #
     # the reason for this is that in the project thesis from which
     # PyTrajectory emerged, the author didn't use the standard approach
     # usually used when dealing with spline interpolation
     #
     # later this standard approach has been implemented and was intended
     # to replace the other one, but it turned out that when using this
     # standard approach some examples suddendly fail
     #
     # until this issue is resolved the user is enabled to choose between
     # the two approaches be altering the following attribute
     self._use_std_approach = use_std_approach
     
     # interval boundaries
     assert a < b
     self.a = a
     self.b = b
     
     # number of polynomial parts
     self.n = int(n)
     
     # 'name' of the spline
     self.tag = tag
     
     # dictionary with boundary values
     #   key: order of the spline's derivative to which the values belong
     #   values: the boundary values the derivative should satisfy
     self._boundary_values = bv
     
     # create array of symbolic coefficients
     self._coeffs = sp.symarray('c'+tag, (self.n, 4))
     self._coeffs_sym = self._coeffs.copy()
     
     # calculate nodes of the spline
     self.nodes = get_spline_nodes(self.a, self.b, self.n+1, nodes_type='equidistant')
     self._nodes_type = 'equidistant' #nodes_type
     
     # size of each polynomial part
     self._h = (self.b - self.a) / float(self.n)
     
     # the polynomial spline parts
     #   key: spline part
     #   value: corresponding polynomial
     self._P = dict()
     for i in xrange(self.n):
         # create polynomials, e.g. for cubic spline:
         #   P_i(t)= c_i_3*t^3 + c_i_2*t^2 + c_i_1*t + c_i_0
         self._P[i] = np.poly1d(self._coeffs[i])
     
     # initialise array for provisionally evaluation of the spline
     # if there are no values for its free parameters
     # 
     # they show how the spline coefficients depend on the free coefficients
     self._dep_array = None  #np.array([])
     self._dep_array_abs = None  #np.array([])
     
     # steady flag is True if smoothness and boundary conditions are solved
     # --> make_steady()
     self._steady_flag = False
     
     # provisionally flag is True as long as there are no numerical values
     # for the free parameters of the spline
     # --> set_coefficients()
     self._prov_flag = True
     
     # the free parameters of the spline
     self._indep_coeffs = None #np.array([])

def n_bar_pendulum(N=1, param_values=dict()):
    '''
    Returns the mass matrix :math:`M` and right hand site :math:`B` of motion equations
    
    .. math::
       M * (d^2/dt^2) x = B
    
    for the :math:`N`\ -bar pendulum.

    Parameters
    ----------
    
    N : int
        Number of bars.
    
    param_values : dict
        Numeric values for the system parameters, 
        such as lengths, masses and gravitational acceleration.
    
    Returns
    -------
    
    sympy.Matrix
        The mass matrix `M`
    
    sympy.Matrix
        The right hand site `B`
    
    list
        List of symbols for state variables
    
    list
        List with symbol for input variable
    '''
    
    # first we have to create some symbols
    F = sp.Symbol('F')                      # the force that acts on the car
    g = sp.Symbol('g')                      # the gravitational acceleration
    m = sp.symarray('m', N+1)               # masses of the car (`m0`) and the bars
    l = sp.symarray('l', N+1)#[1:]          # length of the bars (`l0` is not needed nor used)
    phi = sp.symarray('phi', N+1)#[1:]      # deflaction angles of the bars (`phi0` is not needed nor used)
    dphi = sp.symarray('dphi', N+1)#[1:]    # 1st derivative of the deflaction angles (`dphi0` is not needed nor used)
    
    if param_values.has_key('F'):
        F = param_values['F']
    elif param_values.has_key(F):
        F = param_values[F]
    
    if param_values.has_key('g'):
        g = param_values['g']
    elif param_values.has_key(g):
        g = param_values[g]
    else:
        g = 9.81
    
    for i, mi in enumerate(m):
        if param_values.has_key(mi.name):
            m[i] = param_values[mi.name]
        elif param_values.has_key(mi):
            m[i] = param_values[mi]
    
    for i, li in enumerate(l):
        if param_values.has_key(li.name):
            l[i] = param_values[li.name]
        elif param_values.has_key(li):
            l[i] = param_values[li]
    
    C = np.empty((N,N), dtype=object)
    S = np.empty((N,N), dtype=object)
    I = np.empty((N), dtype=object)
    for i in xrange(1,N+1):
        for j in xrange(1,N+1):
            C[i-1,j-1] = cos(phi[i] - phi[j])
            S[i-1,j-1] = sin(phi[i] - phi[j])
    
    for i in xrange(1,N+1):
        if param_values.has_key('I_%d'%i):
            I[i-1] = param_values['I_%d'%i]
        #elif param_values.has_key(Ii):
        #    I[i] = param_values[Ii]
        else:
            I[i-1] = 4.0/3.0 * m[i] * l[i]**2
    
    #-------------#
    # Mass matrix #
    #-------------#
    M = np.empty((N+1, N+1), dtype=object)

    # 1st row
    M[0,0] = m.sum()
    for j in xrange(1,N):
        M[0,j] = (m[j] + 2*m[j+1:].sum()) * l[j] * cos(phi[j])
    M[0,N] = m[N] * l[N] * cos(phi[N])

    # rest of upper triangular part, except last column
    for i in xrange(1,N):
        M[i,i] = I[i-1] + (m[i] + 4.0*m[i+1:].sum()) * l[i]**2
        for j in xrange(i+1,N):
            M[i,j] = 2.0*(m[j] + 2.0*m[j+1:].sum())*l[i]*l[j]*C[j-1,i-1]

#.........这里部分代码省略.........