def _compute_joint_acceleration(model, robo, j, i):
    """
    Compute the joint acceleration for joint j.

    Args:
        model: An instance of DynModel
        robo: An instance of Robot
        j: link number
        i: antecedent value

    Returns:
        An instance of DynModel that contains all the new values.
    """
    # local variables
    j_a_j = robo.geos[j].axisa
    j_s_i = robo.geos[j].tmat.s_j_wrt_i
    j_gamma_j = model.gammas[j].val
    j_inertia_j_s = model.star_inertias[j].val
    j_beta_j_s = model.star_betas[j].val
    h_j = Matrix([model.joint_inertias[j]])
    tau_j = Matrix([model.taus[j]])
    i_vdot_i = model.accels[i].val
    # actual computation
    qddot_j = h_j.inv() * (-j_a_j.transpose() * j_inertia_j_s * \
        ((j_s_i * i_vdot_i) + j_gamma_j) + tau_j + \
        (j_a_j.transpose() * j_beta_j_s))
    # store in model
    model.qddots[j] = qddot_j[0, 0]
    return model

def generalized_inertia_forces_K(K, q, u, kde_map, vc_map=None):
    """Returns a list of the generalized inertia forces using equation 5.6.6
    from Kane 1985.

    'K' is a potential energy function for the system.
    'q' is a list of generalized coordinates.
    'u' is a list of the independent generalized speeds.
    'kde_map' is a dictionary with q dots as keys and the equivalent
        expressions in terms of q's and u's as values.
    'vc_map' is a dictionay with the dependent u's as keys and the expression
        in terms of independent u's as values.
    """
    n = len(q)
    p = len(u)
    m = n - p
    t = symbols('t')
    if vc_map is None:
        A_kr = Matrix.zeros(m, p)
    else:
        A_kr, _ = vc_matrix(u, vc_map)
        u_ = u + sorted(vc_map.keys(), cmp=lambda x, y: x.compare(y))
    W_sr, _ = kde_matrix(u_, kde_map)

    qd = map(lambda x: x.diff(t), q)
    K_partial_term = [K.diff(qd_s).diff(t) - K.diff(q_s)
                      for qd_s, q_s in zip(qd, q)]
    K_partial_term = subs(K_partial_term, kde_map)
    if vc_map is not None:
        K_partial_term = subs(K_partial_term, vc_map)
    Fr_star = Matrix.zeros(1, p)
    for s in range(n):
        Fr_star -= K_partial_term[s] * (W_sr[s, :p] + W_sr[s, p:]*A_kr[:, :p])
    return Fr_star[:]

def test_extract():
    m = Matrix(4, 3, lambda i, j: i*3 + j)
    assert m.extract([0,1,3],[0,1]) == Matrix(3,2,[0,1,3,4,9,10])
    assert m.extract([0,3],[0,0,2]) == Matrix(2,3,[0,0,2,9,9,11])
    assert m.extract(range(4),range(3)) == m
    raises(IndexError, 'm.extract([4], [0])')
    raises(IndexError, 'm.extract([0], [3])')

    def _kindiffeq(self, kdeqs):
        """Supply all the kinematic differential equations in a list.

        They should be in the form [Expr1, Expr2, ...] where Expri is equal to
        zero

        Parameters
        ==========

        kdeqs : list (of Expr)
            The listof kinematic differential equations

        """
        if len(self._q) != len(kdeqs):
            raise ValueError('There must be an equal number of kinematic '
                             'differential equations and coordinates.')

        uaux = self._uaux
        # dictionary of auxiliary speeds which are equal to zero
        uaz = dict(zip(uaux, [0] * len(uaux)))

        kdeqs = Matrix(kdeqs).subs(uaz)

        qdot = self._qdot
        qdotzero = dict(zip(qdot, [0] * len(qdot)))
        u = self._u
        uzero = dict(zip(u, [0] * len(u)))

        f_k = kdeqs.subs(uzero).subs(qdotzero)
        k_kqdot = (kdeqs.subs(uzero) - f_k).jacobian(Matrix(qdot))
        k_ku = (kdeqs.subs(qdotzero) - f_k).jacobian(Matrix(u))

        self._k_ku = self._mat_inv_mul(k_kqdot, k_ku)
        self._f_k = self._mat_inv_mul(k_kqdot, f_k)
        self._k_kqdot = eye(len(qdot))

def tet4():
    N1 = z
    N2 = n
    N3 = b
    N4 = 1 - z - n - b

    X = Matrix([[1, x, y, z]])
    X2 = Matrix([[1, x1, y1, z1],
                 [1, x2, y2, z2],
                 [1, x3, y3, z3],
                 [1, x4, y4, z4]])
    N = X * X2.inv()
    N1 = N[0, 0]
    N2 = N[0, 1]
    N3 = N[0, 2]
    N4 = N[0, 3]

    N = Matrix([[N1, 0, 0, N2, 0, 0, N3, 0, 0, N4, 0, 0],
                [0, N1, 0, 0, N2, 0, 0, N3, 0, 0, N4, 0],
                [0, 0, N1, 0, 0, N2, 0, 0, N3, 0, 0, N4]])
    NT = N.transpose()
    #pdV = p*v
    pdV = 3
    factorI = 1
    M = makeM(pdV, NT, factorI, levels=3)
    print "Mtet = \n", M

def test_Matrix_sum():
    x, y, z = Symbol('x'), Symbol('y'), Symbol('z')
    M = Matrix([[1,2,3],[x,y,x],[2*y,-50,z*x]])
    m = matrix([[2,3,4],[x,5,6],[x,y,z**2]])
    assert M+m == Matrix([[3,5,7],[2*x,y+5,x+6],[2*y+x,y-50,z*x+z**2]])
    assert m+M == Matrix([[3,5,7],[2*x,y+5,x+6],[2*y+x,y-50,z*x+z**2]])
    assert M+m == M.add(m)

def Newton_solver_pq(error, hx, ht, gx, gt, x0):
    
    p, q  = symbols('p q')
    
    epsilon1 = gx*hx[-2]**p + gt*ht[-2]**q - error[-2]
    epsilon2 = gx*hx[-1]**p + gt*ht[-1]**q - error[-1]


    epsilon = [epsilon1, epsilon2]
    x = [p, q]
    
    def f(i,j):
        return diff(epsilon[i], x[j])
        
    Jacobi = Matrix(2, 2, f)

    JacobiInv = Jacobi.inv()
    epsilonfunc = lambdify(x, epsilon)
    JacobiInvfunc = lambdify(x, JacobiInv)
    x = x0
    
    
    

    F = np.asarray(epsilonfunc(x))
    
    for n in range(8):
        Jinv = np.asarray(JacobiInvfunc(x))
        F = np.asarray(epsilonfunc(x))
        x = x - np.dot(Jinv, F)

    return x

    def etape(self, frontier, i_graph):
        """ Calculates the most probable seed within the infected nodes"""

        # Taking the actual submatrix, not the laplacian matrix. The change
        # lies in the total number of connections (The diagonal terms) for the
        # infected nodes connected to uninfected ones in the initial graph
        i_laplacian_matrix = nx.laplacian_matrix(i_graph)
        for i in range(0, len(i_graph.nodes())):
            if frontier.has_node(i_graph.nodes()[i]):
                i_laplacian_matrix[i, i] +=\
                                frontier.node[i_graph.nodes()[i]]['clear']

        # SymPy
        Lm = Matrix(i_laplacian_matrix.todense())
        i = self.Sym2NumArray(Matrix(Lm.eigenvects()[0][2][0])).argmax()

        # NumPy
        # val, vect = linalg.eigh(i_laplacian_matrix.todense())
        #i = vect[0].argmax()

        # SciPY
        # val, vect = eigs(i_laplacian_matrix.rint())
        # i = vect[:, 0].argmax()

        seed = (i_graph.nodes()[i])

        return seed

def printnp(m):
    """
    Prints a sympy matrix in the same format as a numpy array
    """
    m = Matrix(m)
    if m.shape[1] == 1:
        m = m.reshape(1, m.shape[0])
    elem_len = max(num_chars(d) for d in m.vec())
    if m.shape[0] > 1:
        outstr = '[['
    else:
        outstr = '['
    for i in xrange(m.shape[0]):
        if i:
            outstr += ' ['
        char_count = 0
        for j, elem in enumerate(m[i, :]):
            char_count += elem_len
            if char_count > 77:
                outstr += '\n '
                char_count = elem_len + 2
            # Add spaces
            outstr += ' ' * (elem_len - num_chars(elem) +
                             int(j != 0)) + str(elem)

        if i < m.shape[0] - 1:
            outstr += ']\n'
    if m.shape[0] > 1:
        outstr += ']]'
    else:
        outstr += ']'
    print outstr

def jacobian_dHdV(nK, y, cap, inds):
	# Set up equations of power flow (power at line from nodal voltage) as symbolic equations and
	# calculate the corresponding Jacobian matrix.
	from sympy import symbols, Matrix

	g = Matrix(np.real(y))
	b = Matrix(np.imag(y))

	e_J = symbols("e1:%d"%(nK+1))
	f_J = symbols("f1:%d"%(nK+1))
	hSluV = []

	if "Pl" in inds:
		nPl = np.asarray(inds["Pl"]).astype(int)
		for k in range(nPl.shape[0]):
			i,j = nPl[k,:]
			hSluV.append((e_J[i]**2+f_J[i]**2)*g[i,j]-(e_J[i]*e_J[j]+f_J[i]*f_J[j])*g[i,j]+(e_J[i]*f_J[j]-e_J[j]*f_J[i])*b[i,j])
	if "Ql" in inds:
		nQl = np.asarray(inds["Ql"]).astype(int)
		for k in range(nQl.shape[0]):
			i,j = nQl[k,:]
			hSluV.append(-(e_J[i]**2+f_J[i]**2)*b[i,j]+(e_J[i]*e_J[j]+f_J[i]*f_J[j])*b[i,j]+(e_J[i]*f_J[j]-e_J[j]*f_J[i])*g[i,j]-(e_J[i]**2+f_J[i]**2)*cap[i,j]/2)
	if "Vm" in inds:
		nVk = np.asarray(inds["Vm"]).astype(int)
		for k in range(nVk.shape[0]):
			i = nVk[k]
			hSluV.append((e_J[i]**2+f_J[i]**2)**0.5)
	if len(hSluV)>0:
		hSluV = Matrix(hSluV)
		ef_J = e_J[1:] + f_J[1:]
		JMatrix_dHdV = hSluV.jacobian(ef_J)
		return JMatrix_dHdV, hSluV
	else:
		return None, None

    def _initialize_vectors(self, q_ind, q_dep, u_ind, u_dep, u_aux):
        """Initialize the coordinate and speed vectors."""

        none_handler = lambda x: Matrix(x) if x else Matrix()

        # Initialize generalized coordinates
        q_dep = none_handler(q_dep)
        if not iterable(q_ind):
            raise TypeError('Generalized coordinates must be an iterable.')
        if not iterable(q_dep):
            raise TypeError('Dependent coordinates must be an iterable.')
        q_ind = Matrix(q_ind)
        self._qdep = q_dep
        self._q = Matrix([q_ind, q_dep])
        self._qdot = self._q.diff(dynamicsymbols._t)

        # Initialize generalized speeds
        u_dep = none_handler(u_dep)
        if not iterable(u_ind):
            raise TypeError('Generalized speeds must be an iterable.')
        if not iterable(u_dep):
            raise TypeError('Dependent speeds must be an iterable.')
        u_ind = Matrix(u_ind)
        self._udep = u_dep
        self._u = Matrix([u_ind, u_dep])
        self._udot = self._u.diff(dynamicsymbols._t)
        self._uaux = none_handler(u_aux)

def calculate_dependencies(equations, task_variables, variables_values):
    
    replacements = replace.get_replacements(task_variables)[1]
    transformed_equations = replace.get_transformed_expressions(equations, task_variables)
    
    #prepare data for Numpy manipulations - change variables names to the names of special format - 'x%sy', also 
    #convert some strings to Symbols so they could be processed by Sympy and Numpy libs
        
    symbolized_replaced_task_variables = sorted(symbols(replacements.keys()))        
    replaced_variables_values = {}
    for k,v in replacements.iteritems():
        replaced_variables_values[Symbol(k)] = variables_values[v]
        
    #prepare the system of equations for solving - find Jacobian, in Jacobian matrix that was found replace variables by their values
    
    F = Matrix(transformed_equations)
    J = F.jacobian(symbolized_replaced_task_variables).evalf(subs=replaced_variables_values)
    X = np.transpose(np.array(J))
    
    #solve the system
    n,m = X.shape
    if (n != m - 1): 
        # if the system of equations is overdetermined (the number of equations is bigger than number of variables 
        #then we have to make regular, square systems of equations out of it (we do that by grabbing only some of the equations)
        square_systems = suggest_square_systems_list(X, n, m)
        for sq in square_systems:
            try:
                solve_system(sq, equations)
            except Exception as e:
                print e.args
                print sq
                
                
    else:
        solve_system(X, equations)  

    def get_dixon_polynomial(self):
        r"""
        Returns
        =======

        dixon_polynomial: polynomial
            Dixon's polynomial is calculated as:

            delta = Delta(A) / ((x_1 - a_1) ... (x_n - a_n)) where,

            A =  |p_1(x_1,... x_n), ..., p_n(x_1,... x_n)|
                 |p_1(a_1,... x_n), ..., p_n(a_1,... x_n)|
                 |...             , ...,              ...|
                 |p_1(a_1,... a_n), ..., p_n(a_1,... a_n)|
        """
        if self.m != (self.n + 1):
            raise ValueError('Method invalid for given combination.')

        # First row
        rows = [self.polynomials]

        temp = list(self.variables)

        for idx in range(self.n):
            temp[idx] = self.dummy_variables[idx]
            substitution = {var: t for var, t in zip(self.variables, temp)}
            rows.append([f.subs(substitution) for f in self.polynomials])

        A = Matrix(rows)

        terms = zip(self.variables, self.dummy_variables)
        product_of_differences = Mul(*[a - b for a, b in terms])
        dixon_polynomial = (A.det() / product_of_differences).factor()

        return poly_from_expr(dixon_polynomial, self.dummy_variables)[0]

def linear_rref(A, b, Matrix=None, S=None):
    """ Transform a linear system to reduced row-echelon form

    Transforms both the matrix and right-hand side of a linear
    system of equations to reduced row echelon form

    Parameters
    ----------
    A: Matrix-like
        iterable of rows
    b: iterable

    Returns
    -------
    A', b' - transformed versions

    """
    if Matrix is None:
        from sympy import Matrix
    if S is None:
        from sympy import S
    mat_rows = [_map2l(S, list(row) + [v]) for row, v in zip(A, b)]
    aug = Matrix(mat_rows)
    raug, pivot = aug.rref()
    nindep = len(pivot)
    return raug[:nindep, :-1], raug[:nindep, -1]

def spin_vector_to_state(spin):
    from sympy import Matrix
    from sglib_v0 import sigma_x, sigma_y, sigma_z, find_eigenvectors
    from numpy import shape
    # The spin vector must have three components.
    if shape(spin) == () or len(spin) != 3:
        print('Error: spin vector must have three elements.')
        return
        
    # Make sure the values are floats and make sure it's really
    # a column vector.
    spin = [ float(spin[0]), float(spin[1]), float(spin[2]) ]
    spin = Matrix(spin)
    
    # Make sure its normalized. Do this silently, since it will
    # probably happen most of the time due to imprecision.
    if spin.norm() != 1.0: spin = spin/spin.norm()
    
    # Calculate the measurement operator as a weighted combination
    # of the three Pauli matrices. 
    O_1 = spin[0]*sigma_x + spin[1]*sigma_y + spin[2]*sigma_z
    O_1.simplify()
    
    # the eigenvectors will be the two possible states. The
    # "minus" state will be the first one.
    evals, evecs = find_eigenvectors(O_1)
    plus_state = evecs[1]
    minus_state = evecs[0]
    return(spin, plus_state, minus_state)

def automaton_growth_func( automaton, initial_state, variable='z' ):
    """Returns Z-transform of the growth function of this automaton
    Uses SYmpy for calculations"""
    from sympy import Symbol, Matrix, eye, cancel
    z = variable if isinstance(variable,Symbol) else Symbol(variable)
    
    n  = automaton.num_states()
    a_,b_,c_ = growth_matrices(automaton, initial_state)
    #a.print_nonzero()
    c = Matrix( 1, n, c_)
    b = Matrix( n, 1, b_)
    a = Matrix( a_)
    del a_, b_, c_
    
    #(z*eye(n)-a).inv().print_nonzero()

    if False: #naiive implementation
        q = z*eye(n)-a
        f = c * q.LUsolve(b) * z
        assert f.shape == (1,1)
        return f[0,0]
    
    if True:
        #use trick...
        den = a.berkowitz_charpoly(z)
        #print("#### charpoly:", den)
        num = (a - b*c).berkowitz_charpoly(z) - den
        #print("#### numerator:", num)
        return num/den

    def __init__(self, frame):
        """Supply the inertial frame for Kane initialization. """
        # Big storage things
        self._inertial = frame
        self._forcelist = None
        self._bodylist = None
        self._fr = None
        self._frstar = None
        self._rhs = None
        self._aux_eq = None

        # States
        self._q = None
        self._qdep = []
        self._qdot = None
        self._u = None
        self._udep = []
        self._udot = None
        self._uaux = None

        # Differential Equations Matrices
        self._k_d = None
        self._f_d = None
        self._k_kqdot = None
        self._k_ku = None
        self._f_k = None

        # Constraint Matrices
        self._f_h = Matrix([])
        self._k_nh = Matrix([])
        self._f_nh = Matrix([])
        self._k_dnh = Matrix([])
        self._f_dnh = Matrix([])

    def test_matrix_tensor_product():
        l1 = zeros(4)
        for i in range(16):
            l1[i] = 2**i
        l2 = zeros(4)
        for i in range(16):
            l2[i] = i
        l3 = zeros(2)
        for i in range(4):
            l3[i] = i
        vec = Matrix([1,2,3])

        #test for Matrix known 4x4 matricies
        numpyl1 = np.matrix(l1.tolist())
        numpyl2 = np.matrix(l2.tolist())
        numpy_product = np.kron(numpyl1,numpyl2)
        args = [l1, l2]
        sympy_product = matrix_tensor_product(*args)
        assert numpy_product.tolist() == sympy_product.tolist()
        numpy_product = np.kron(numpyl2,numpyl1)
        args = [l2, l1]
        sympy_product = matrix_tensor_product(*args)
        assert numpy_product.tolist() == sympy_product.tolist()

        #test for other known matrix of different dimensions
        numpyl2 = np.matrix(l3.tolist())
        numpy_product = np.kron(numpyl1,numpyl2)
        args = [l1, l3]
        sympy_product = matrix_tensor_product(*args)
        assert numpy_product.tolist() == sympy_product.tolist()
        numpy_product = np.kron(numpyl2,numpyl1)
        args = [l3, l1]
        sympy_product = matrix_tensor_product(*args)
        assert numpy_product.tolist() == sympy_product.tolist()

        #test for non square matrix
        numpyl2 = np.matrix(vec.tolist())
        numpy_product = np.kron(numpyl1,numpyl2)
        args = [l1, vec]
        sympy_product = matrix_tensor_product(*args)
        assert numpy_product.tolist() == sympy_product.tolist()
        numpy_product = np.kron(numpyl2,numpyl1)
        args = [vec, l1]
        sympy_product = matrix_tensor_product(*args)
        assert numpy_product.tolist() == sympy_product.tolist()

        #test for random matrix with random values that are floats
        random_matrix1 = np.random.rand(np.random.rand()*5+1,np.random.rand()*5+1)
        random_matrix2 = np.random.rand(np.random.rand()*5+1,np.random.rand()*5+1)
        numpy_product = np.kron(random_matrix1,random_matrix2)
        args = [Matrix(random_matrix1.tolist()),Matrix(random_matrix2.tolist())]
        sympy_product = matrix_tensor_product(*args)
        assert not (sympy_product - Matrix(numpy_product.tolist())).tolist() > \
        (ones((sympy_product.rows,sympy_product.cols))*epsilon).tolist()

        #test for three matrix kronecker
        sympy_product = matrix_tensor_product(l1,vec,l2)

        numpy_product = np.kron(l1,np.kron(vec,l2))
        assert numpy_product.tolist() == sympy_product.tolist()

def Newton_solver_gxgt(error, hx, ht, x0, p_value=1, q_value=1):
    
    gx, gt, p, q, hx1, hx2, ht1, ht2  = symbols('gx gt p q qhx1 hx2 ht1 ht2')
    epsilon1 = gx*hx1**p + gt*ht1**q - error[-2]
    epsilon2 = gx*hx2**p + gt*ht2**q - error[-1]


    epsilon = [epsilon1, epsilon2]
    x = [gx, gt]
    knowns = [p, q, hx1, hx2, ht1, ht2]
    
    def f(i,j):
        return diff(epsilon[i], x[j])
        
    Jacobi = Matrix(2, 2, f)

    JacobiInv = Jacobi.inv()
    epsilonfunc = lambdify([x, knowns], epsilon)
    JacobiInvfunc = lambdify([x, knowns], JacobiInv)
    x = x0
    knowns = [p_value, q_value, hx[-2], hx[-1], ht[-2], ht[-1]]
    F = np.asarray(epsilonfunc(x, knowns))
    
    for n in range(8):
        Jinv = np.asarray(JacobiInvfunc(x, knowns))
        F = np.asarray(epsilonfunc(x, knowns))
        x = x - np.dot(Jinv, F)

        print "n, x: ", n, x
    
    return x

def compute_lambda(robo, symo, j, antRj, antPj, lam):
    """Internal function. Computes the inertia parameters
    transformation matrix

    Notes
    =====
    lam is the output paramete
    """
    lamJJ_list = []
    lamJMS_list = []
    for e1 in xrange(3):
        for e2 in xrange(e1, 3):
            u = vec_mut_J(antRj[j][:, e1], antRj[j][:, e2])
            if e1 != e2:
                u += vec_mut_J(antRj[j][:, e2], antRj[j][:, e1])
            lamJJ_list.append(u.T)
    for e1 in xrange(3):
        v = vec_mut_MS(antRj[j][:, e1], antPj[j])
        lamJMS_list.append(v.T)
    lamJJ = Matrix(lamJJ_list).T  # , 'LamJ', j)
    lamJMS = symo.mat_replace(Matrix(lamJMS_list).T, 'LamMS', j)
    lamJM = symo.mat_replace(vec_mut_M(antPj[j]), 'LamM', j)
    lamJ = lamJJ.row_join(lamJMS).row_join(lamJM)
    lamMS = sympy.zeros(3, 6).row_join(antRj[j]).row_join(antPj[j])
    lamM = sympy.zeros(1, 10)
    lamM[9] = 1
    lam[j] = Matrix([lamJ, lamMS, lamM])