def test_quaternion_conversions():
    q1 = Quaternion(1, 2, 3, 4)

    assert q1.to_axis_angle() == ((2 * sqrt(29)/29,
                                   3 * sqrt(29)/29,
                                   4 * sqrt(29)/29),
                                   2 * acos(sqrt(30)/30))

    assert q1.to_rotation_matrix() == Matrix([[-S(2)/3, S(2)/15, S(11)/15],
                                     [S(2)/3, -S(1)/3, S(14)/15],
                                     [S(1)/3, S(14)/15, S(2)/15]])

    assert q1.to_rotation_matrix((1, 1, 1)) == Matrix([[-S(2)/3, S(2)/15, S(11)/15, S(4)/5],
                                                  [S(2)/3, -S(1)/3, S(14)/15, -S(4)/15],
                                                  [S(1)/3, S(14)/15, S(2)/15, -S(2)/5],
                                                  [S(0), S(0), S(0), S(1)]])

    theta = symbols("theta", real=True)
    q2 = Quaternion(cos(theta/2), 0, 0, sin(theta/2))

    assert trigsimp(q2.to_rotation_matrix()) == Matrix([
                                               [cos(theta), -sin(theta), 0],
                                               [sin(theta),  cos(theta), 0],
                                               [0,           0,          1]])

    assert q2.to_axis_angle() == ((0, 0, sin(theta/2)/Abs(sin(theta/2))),
                                   2*acos(cos(theta/2)))

    assert trigsimp(q2.to_rotation_matrix((1, 1, 1))) == Matrix([
               [cos(theta), -sin(theta), 0, sin(theta) - cos(theta) + 1],
               [sin(theta),  cos(theta), 0, -sin(theta) - cos(theta) + 1],
               [0,           0,          1,  0],
               [0,           0,          0,  1]])

def _solve_type_2(symo, X, Y, Z, th):
    """Solution for the equation:
    X*S + Y*C = Z
    """
    symo.write_line("# X*sin({0}) + Y*cos({0}) = Z".format(th))
    X = symo.replace(trigsimp(X), 'X', th)
    Y = symo.replace(trigsimp(Y), 'Y', th)
    Z = symo.replace(trigsimp(Z), 'Z', th)
    YPS = var('YPS'+str(th))
    if X == tools.ZERO and Y != tools.ZERO:
        C = symo.replace(Z/Y, 'C', th)
        symo.add_to_dict(YPS, (tools.ONE, - tools.ONE))
        symo.add_to_dict(th, atan2(YPS*sqrt(1-C**2), C))
    elif X != tools.ZERO and Y == tools.ZERO:
        S = symo.replace(Z/X, 'S', th)
        symo.add_to_dict(YPS, (tools.ONE, - tools.ONE))
        symo.add_to_dict(th, atan2(S, YPS*sqrt(1-S**2)))
    elif Z == tools.ZERO:
        symo.add_to_dict(YPS, (tools.ONE, tools.ZERO))
        symo.add_to_dict(th, atan2(-Y, X) + YPS*pi)
    else:
        B = symo.replace(X**2 + Y**2, 'B', th)
        D = symo.replace(B - Z**2, 'D', th)
        symo.add_to_dict(YPS, (tools.ONE, - tools.ONE))
        S = symo.replace((X*Z + YPS * Y * sqrt(D))/B, 'S', th)
        C = symo.replace((Y*Z - YPS * X * sqrt(D))/B, 'C', th)
        symo.add_to_dict(th, atan2(S, C))

def test_trigsimp_groebner():
    from sympy.simplify.trigsimp import trigsimp_groebner

    c = cos(x)
    s = sin(x)
    ex = (4*s*c + 12*s + 5*c**3 + 21*c**2 + 23*c + 15)/(
        -s*c**2 + 2*s*c + 15*s + 7*c**3 + 31*c**2 + 37*c + 21)
    resnum = (5*s - 5*c + 1)
    resdenom = (8*s - 6*c)
    results = [resnum/resdenom, (-resnum)/(-resdenom)]
    assert trigsimp_groebner(ex) in results
    assert trigsimp_groebner(s/c, hints=[tan]) == tan(x)
    assert trigsimp_groebner(c*s) == c*s
    assert trigsimp((-s + 1)/c + c/(-s + 1),
                    method='groebner') == 2/c
    assert trigsimp((-s + 1)/c + c/(-s + 1),
                    method='groebner', polynomial=True) == 2/c

    # Test quick=False works
    assert trigsimp_groebner(ex, hints=[2]) in results

    # test "I"
    assert trigsimp_groebner(sin(I*x)/cos(I*x), hints=[tanh]) == I*tanh(x)

    # test hyperbolic / sums
    assert trigsimp_groebner((tanh(x)+tanh(y))/(1+tanh(x)*tanh(y)),
                             hints=[(tanh, x, y)]) == tanh(x + y)

def test_issue_15129_trigsimp_methods():
    t1 = Matrix([sin(Rational(1, 50)), cos(Rational(1, 50)), 0])
    t2 = Matrix([sin(Rational(1, 25)), cos(Rational(1, 25)), 0])
    t3 = Matrix([cos(Rational(1, 25)), sin(Rational(1, 25)), 0])
    r1 = t1.dot(t2)
    r2 = t1.dot(t3)
    assert trigsimp(r1) == cos(S(1)/50)
    assert trigsimp(r2) == sin(S(3)/50)

def test_trigsimp2():
    x, y = symbols('x,y')
    assert trigsimp(cos(x)**2*sin(y)**2 + cos(x)**2*cos(y)**2 + sin(x)**2,
            recursive=True) == 1
    assert trigsimp(sin(x)**2*sin(y)**2 + sin(x)**2*cos(y)**2 + cos(x)**2,
            recursive=True) == 1
    assert trigsimp(
        Subs(x, x, sin(y)**2 + cos(y)**2)) == Subs(x, x, 1)

def test_issue_4661():
    a, x, y = symbols('a x y')
    eq = -4*sin(x)**4 + 4*cos(x)**4 - 8*cos(x)**2
    assert trigsimp(eq) == -4
    n = sin(x)**6 + 4*sin(x)**4*cos(x)**2 + 5*sin(x)**2*cos(x)**4 + 2*cos(x)**6
    d = -sin(x)**2 - 2*cos(x)**2
    assert simplify(n/d) == -1
    assert trigsimp(-2*cos(x)**2 + cos(x)**4 - sin(x)**4) == -1
    eq = (- sin(x)**3/4)*cos(x) + (cos(x)**3/4)*sin(x) - sin(2*x)*cos(2*x)/8
    assert trigsimp(eq) == 0

    def test_transformation(self):

        theta = sympy.Symbol("theta")
        trans = matrices.TTransX(theta)
        trans_1 = matrices.TTransX(-theta)
        self.assertEqual(sympy.trigsimp(trans.m * trans_1.m), sympy.Matrix.eye(4))

        rot = matrices.TRotX(theta)
        rot_1 = matrices.TRotX(-theta)
        self.assertEqual(sympy.trigsimp(rot.m * rot_1.m), sympy.Matrix.eye(4))

 def _w_diff_dcm(self, otherframe):
     """Angular velocity from time differentiating the DCM. """
     from sympy.physics.vector.functions import dynamicsymbols
     dcm2diff = self.dcm(otherframe)
     diffed = dcm2diff.diff(dynamicsymbols._t)
     angvelmat = diffed * dcm2diff.T
     w1 = trigsimp(expand(angvelmat[7]), recursive=True)
     w2 = trigsimp(expand(angvelmat[2]), recursive=True)
     w3 = trigsimp(expand(angvelmat[3]), recursive=True)
     return -Vector([(Matrix([w1, w2, w3]), self)])

def solve_orientation(robo, symo, pieper_joints):
    """
    Function that solves the orientation equation for the four spherical cases.

    Parameters:
    ===========
    1) pieper_joints: Joints that form the spherical wrist
    """
    m = pieper_joints[1]        # Center of the spherical joint
    [S,N,A] = [0,1,2]           # Book convention indexes
    [x,y,z] = [0,1,2]           # Book convention indexes

    t1 = dgm(robo, symo, m-2, 0, fast_form=True, trig_subs=True)
    t2 = dgm(robo, symo, 6, m+1, fast_form=True, trig_subs=True)

    Am2A0 = Matrix([ t1[:3,:3] ])
    A6Am1 = Matrix([ t2[:3,:3] ])

    A0 = T_GENERAL[:3,:3]

    SNA = _rot(axis=x, th=-robo.alpha[m-1])*Am2A0*A0*A6Am1
    SNA = symo.replace(trigsimp(SNA), 'SNA')

    # calculate theta(m-1) (i)
    # eq 1.68
    eq_type = 3
    offset = robo.gamma[robo.ant[m-1]]
    robo.r[m-1] = robo.r[m-1] + robo.b[robo.ant[m-1]]
    coef = [-SNA[y,A]*sin(robo.alpha[m]) , SNA[x,A]*sin(robo.alpha[m]) , SNA[z,A]*cos(robo.alpha[m])-cos(robo.alpha[m+1])]
    _equation_solve(symo, coef, eq_type, robo.theta[m-1], offset)

    # calculate theta(m) (j)
    # eq 1.72
    S1N1A1 = _rot(axis=x, th=-robo.alpha[m])*_rot(axis=z, th=-robo.theta[m-1])*SNA
    eq_type = 4
    offset = robo.gamma[robo.ant[m]]
    robo.r[m] = robo.r[m] + robo.b[robo.ant[m]]
    symo.write_line("\r\n\r\n")
    B1 = symo.replace(trigsimp(-S1N1A1[x,A]), 'B1', robo.theta[m])
    B2 = symo.replace(trigsimp(S1N1A1[y,A]), 'B2', robo.theta[m])
    coef = [0, sin(robo.alpha[m+1]), B1, sin(robo.alpha[m+1]), 0, B2]
    _equation_solve(symo, coef, eq_type, robo.theta[m], offset)

    # calculate theta(m+1) (k)
    # eq 1.73
    eq_type = 4
    offset = robo.gamma[robo.ant[m+1]]
    robo.r[m+1] = robo.r[m+1] + robo.b[robo.ant[m+1]]
    symo.write_line("\r\n\r\n")
    B1 = symo.replace(trigsimp(-S1N1A1[z,S]), 'B1', robo.theta[m+1])
    B2 = symo.replace(trigsimp(-S1N1A1[z,N]), 'B2', robo.theta[m+1])
    coef = [0, sin(robo.alpha[m+1]), B1, sin(robo.alpha[m+1]), 0, B2]
    _equation_solve(symo, coef, eq_type, robo.theta[m+1], offset)

    return

 def _w_diff_dcm(self, otherframe):
     """Angular velocity from time differentiating the DCM. """
     from sympy.physics.vector.functions import dynamicsymbols
     dcm2diff = self.dcm(otherframe)
     diffed = dcm2diff.diff(dynamicsymbols._t)
     # angvelmat = diffed * dcm2diff.T
     # This one seems to produce the correct result when I checked using Autolev.
     angvelmat = dcm2diff*diffed.T
     w1 = trigsimp(expand(angvelmat[7]), recursive=True)
     w2 = trigsimp(expand(angvelmat[2]), recursive=True)
     w3 = trigsimp(expand(angvelmat[3]), recursive=True)
     return -Vector([(Matrix([w1, w2, w3]), self)])

def _try_solve_0(symo, eq_sys, knowns):
    res = False
    for eq, [r], th_names in eq_sys:
        X = tools.get_max_coef(eq, r)
        if X != 0:
            Y = X*r - eq
            symo.write_line("# Solving type 1")
            X = symo.replace(trigsimp(X), 'X', r)
            Y = symo.replace(trigsimp(Y), 'Y', r)
            symo.add_to_dict(r, Y/X)
            knowns.add(r)
            res = True
    return res

def _solve_type_4(symo, X1, Y1, X2, Y2, th, r):
    """Solution for the system:
    X1*S*r = Y1
    X2*C*r = Y2
    """
    symo.write_line("# X1*sin({0})*{1} = Y1".format(th, r))
    symo.write_line("# X2*cos({0})*{1} = Y2".format(th, r))
    X1 = symo.replace(trigsimp(X1), 'X1', th)
    Y1 = symo.replace(trigsimp(Y1), 'Y1', th)
    X2 = symo.replace(trigsimp(X2), 'X2', th)
    Y2 = symo.replace(trigsimp(Y2), 'Y2', th)
    YPS = var('YPS' + r)
    symo.add_to_dict(YPS, (tools.ONE, - tools.ONE))
    symo.add_to_dict(r, YPS*sqrt((Y1/X1)**2 + (Y2/X2)**2))
    symo.add_to_dict(th, atan2(Y1/(X1*r), Y2/(X2*r)))

def test_trigsimp3():
    x, y = symbols('x,y')
    assert trigsimp(sin(x)/cos(x)) == tan(x)
    assert trigsimp(sin(x)**2/cos(x)**2) == tan(x)**2
    assert trigsimp(sin(x)**3/cos(x)**3) == tan(x)**3
    assert trigsimp(sin(x)**10/cos(x)**10) == tan(x)**10

    assert trigsimp(cos(x)/sin(x)) == 1/tan(x)
    assert trigsimp(cos(x)**2/sin(x)**2) == 1/tan(x)**2
    assert trigsimp(cos(x)**10/sin(x)**10) == 1/tan(x)**10

    assert trigsimp(tan(x)) == trigsimp(sin(x)/cos(x))

    def scalar_map(self, other):
        """
        Returns a dictionary which expresses the coordinate variables
        (base scalars) of this frame in terms of the variables of
        otherframe.

        Parameters
        ==========

        otherframe : CoordSysCartesian
            The other system to map the variables to.

        Examples
        ========

        >>> from sympy.vector import CoordSysCartesian
        >>> from sympy import Symbol
        >>> A = CoordSysCartesian('A')
        >>> q = Symbol('q')
        >>> B = A.orient_new('B', 'Axis', [q, A.k])
        >>> A.scalar_map(B)
        {A.x: B.x*cos(q) - B.y*sin(q), A.y: B.x*sin(q) + B.y*cos(q), A.z: B.z}

        """

        vars_matrix = self.rotation_matrix(other) * Matrix(other.base_scalars())
        mapping = {}
        for i, x in enumerate(self.base_scalars()):
            mapping[x] = trigsimp(vars_matrix[i])
        return mapping

    def _generate_eoms(self):

        self.kane = me.KanesMethod(self.frames['inertial'],
                                   self.coordinates.values(),
                                   self.speeds.values(),
                                   self.kin_diff_eqs)

        fr, frstar = self.kane.kanes_equations(self.loads.values(),
                                               self.rigid_bodies.values())

        sub = {self.specified['platform_speed'].diff(self.time):
               self.specified['platform_acceleration']}

        self.fr_plus_frstar = sy.trigsimp(fr + frstar).subs(sub)

        udots = sy.Matrix([s.diff(self.time) for s in self.speeds.values()])

        m1 = self.fr_plus_frstar.diff(udots[0])
        m2 = self.fr_plus_frstar.diff(udots[1])

        # M x' = F
        self.mass_matrix = -m1.row_join(m2)
        self.forcing_vector = sy.simplify(self.fr_plus_frstar +
                                          self.mass_matrix * udots)

        M_top_rows = sy.eye(2).row_join(sy.zeros(2))
        F_top_rows = sy.Matrix(self.speeds.values())

        tmp = sy.zeros(2).row_join(self.mass_matrix)
        self.mass_matrix_full = M_top_rows.col_join(tmp)
        self.forcing_vector_full = F_top_rows.col_join(self.forcing_vector)

def square_root_of_expr(expr):
    """
    If expression is product of even powers then every power is divided
    by two and the product is returned.  If some terms in product are
    not even powers the sqrt of the absolute value of the expression is
    returned.  If the expression is a number the sqrt of the absolute
    value of the number is returned.
    """
    if expr.is_number:
        if expr > 0:
            return(sqrt(expr))
        else:
            return(sqrt(-expr))
    else:
        expr = trigsimp(expr)
        (coef, pow_lst) = sqf_list(expr)
        if coef != S(1):
            if coef.is_number:
                coef = square_root_of_expr(coef)
            else:
                coef = sqrt(abs(coef))  # Product coefficient not a number
        for p in pow_lst:
            (f, n) = p
            if n % 2 != 0:
                return(sqrt(abs(expr)))  # Product not all even powers
            else:
                coef *= f ** (n / 2)  # Positive sqrt of the square of an expression
        return coef

def solve_orientation_prismatic(robo, symo, X_joints):
    """
    Function that solves the orientation equation of the three prismatic joints case. (to find the three angles)

    Parameters:
    ===========
    1) X_joint: The three revolute joints for the prismatic case
    """
    [i,j,k] = X_joints                   # X joints vector
    [S,S1,S2,S3,x] = [0,0,0,0,0]        # Book convention indexes
    [N,N1,N2,N3,y] = [1,1,1,1,1]        # Book convention indexes
    [A,A1,A2,A3,z] = [2,2,2,2,2]        # Book convention indexes
    robo.theta[i] = robo.theta[i] + robo.gamma[robo.ant[i]]
    robo.theta[j] = robo.theta[j] + robo.gamma[robo.ant[j]]
    robo.theta[k] = robo.theta[k] + robo.gamma[robo.ant[k]]

    T6k = dgm(robo, symo, 6, k, fast_form=True,trig_subs=True)
    Ti0 = dgm(robo, symo, robo.ant[i], 0, fast_form=True, trig_subs=True)
    Tji = dgm(robo, symo, j-1, i, fast_form=True, trig_subs=True)
    Tjk = dgm(robo, symo, j, k-1, fast_form=True, trig_subs=True)

    S3N3A3 = _rot_trans(axis=x, th=-robo.alpha[i], p=0)*Ti0*T_GENERAL*T6k   # S3N3A3 = rot(x,-alphai)*rot(z,-gami)*aiTo*SNA
    S2N2A2 = _rot_trans(axis=x, th=-robo.alpha[j], p=0)*Tji                 # S2N2A2 = iTa(j)*rot(x,-alphaj)
    S1N1A1 = Tjk*_rot_trans(axis=x, th=-robo.alpha[k], p=0)                 # S1N1A1 = jTa(k)*rot(x,alphak)

    S3N3A3 = Matrix([ S3N3A3[:3, :3] ])
    S2N2A2 = Matrix([ S2N2A2[:3, :3] ])
    S1N1A1 = Matrix([ S1N1A1[:3, :3] ])
    SNA = Matrix([ T_GENERAL[:3, :3] ])
    SNA = symo.replace(trigsimp(SNA), 'SNA')

    # solve thetai
    # eq 1.100 (page 49)
    eq_type = 3
    offset = robo.gamma[robo.ant[i]]
    robo.r[i] = robo.r[i] + robo.b[robo.ant[i]]
    el1 = S2N2A2[z,S2]*S3N3A3[x,A3] + S2N2A2[z,N2]*S3N3A3[y,A3]
    el2 = S2N2A2[z,S2]*S3N3A3[y,A3] - S2N2A2[z,N2]*S3N3A3[x,A3]
    el3 = S2N2A2[z,A2]*S3N3A3[z,A3] - S1N1A1[z,A1]
    coef = [el1,el2,el3]
    _equation_solve(symo, coef, eq_type, robo.theta[i], offset)

    # solve thetaj
    # eq 1.102
    eq_type = 4
    offset = robo.gamma[robo.ant[j]]
    robo.r[j] = robo.r[j] + robo.b[robo.ant[j]]
    coef = [S1N1A1[x,A1] , -S1N1A1[y,A1] , -SNA[x,A] , S1N1A1[y,A1] , S1N1A1[x,A1] , -SNA[y,A] ]
    _equation_solve(symo, coef, eq_type, robo.theta[j], offset)

    # solve thetak
    # eq 1.103
    eq_type = 4
    offset = robo.gamma[robo.ant[k]]
    robo.r[k] = robo.r[k] + robo.b[robo.ant[k]]
    coef = [S1N1A1[z,S1] , S1N1A1[z,N1] , -SNA[z,S] , S1N1A1[z,N1] , -S1N1A1[z,S1] , -SNA[z,N] ]
    _equation_solve(symo, coef, eq_type, robo.theta[k], offset)

    return

def test_issue_2827_trigsimp_methods():
    measure1 = lambda expr: len(str(expr))
    measure2 = lambda expr: -count_ops(expr)
                                       # Return the most complicated result
    expr = (x + 1)/(x + sin(x)**2 + cos(x)**2)
    ans = Matrix([1])
    M = Matrix([expr])
    assert trigsimp(M, method='fu', measure=measure1) == ans
    assert trigsimp(M, method='fu', measure=measure2) != ans
    # all methods should work with Basic expressions even if they
    # aren't Expr
    M = Matrix.eye(1)
    assert all(trigsimp(M, method=m) == M for m in
        'fu matching groebner old'.split())
    # watch for E in exptrigsimp, not only exp()
    eq = 1/sqrt(E) + E
    assert exptrigsimp(eq) == eq

 def _symplify_expr(self,expr): # this is a costly stage for complex expressions
         """
         Perform simplification of the provided expression.
         Method returns a SymPy expression.
         """
         expr = sympy.trigsimp(expr) 
         expr = sympy.simplify(expr) 
         return expr

def test_issue_6811_fail():
    # from doc/src/modules/physics/mechanics/examples.rst, the current `eq`
    # at Line 576 (in different variables) was formerly the equivalent and
    # shorter expression given below...it would be nice to get the short one
    # back again
    xp, y, x, z = symbols('xp, y, x, z')
    eq = 4*(-19*sin(x)*y + 5*sin(3*x)*y + 15*cos(2*x)*z - 21*z)*xp/(9*cos(x) - 5*cos(3*x))
    assert trigsimp(eq) == -2*(2*cos(x)*tan(x)*y + 3*z)*xp/cos(x)