from sympy import *
#from sympy.abc import *
init_printing(use_latex=True)

from sympy import symbols
from sympy.physics.mechanics import ReferenceFrame, Point, dynamicsymbols

#ejes tierra
Earth = ReferenceFrame("Earth", latexs=['\mathbf{i_{e}}', '\mathbf{j_{e}}', '\mathbf{k_{e}}'])
#ejes horizonte local
Hor = ReferenceFrame("Hor", latexs=['\mathbf{i_{h}}', '\mathbf{j_{h}}', '\mathbf{k_{h}}'])
#ejes cuerpo
Body = ReferenceFrame("Body", latexs=['\mathbf{i_{b}}', '\mathbf{j_{b}}', '\mathbf{k_{b}}'])
#ejes viento
Wind = ReferenceFrame("Wind", latexs=['\mathbf{i_{w}}', '\mathbf{j_{w}}', '\mathbf{k_{w}}'])

psi, theta, phi = dynamicsymbols('psi, theta, phi')
Body.orient(Hor, 'Body', (psi, theta, phi), 'ZYX')
L_bh = Body.dcm(Hor)
L_bh

chi, gamma, mu = dynamicsymbols('chi, \gamma, mu')
Wind.orient(Hor, 'Body', (chi, gamma, mu), 'ZYX')
L_wh = Wind.dcm(Hor)
L_wh

beta, alpha = dynamicsymbols('beta, alpha')
Body.orient(Wind, 'Body', (-beta, alpha, 0), 'ZYX')
L_bw = Body.dcm(Wind)
L_bw

Hor.orient(Earth, 'Body', (0, 0, 0), 'ZYX')
Hor.dcm(Earth)

from sympy.physics.mechanics import inertia
I_x, I_y, I_z, J_xz = symbols('I_x, I_y, I_z, J_xz')
I = inertia(Body, I_x, I_y, I_z, 0, 0, -J_xz)
I

F_x, F_y, F_z = dynamicsymbols('F_x, F_y, F_z')
L, M, N = dynamicsymbols('L, M, N')
u, v, w = dynamicsymbols('u, v, w')
p, q, r = dynamicsymbols('p, q, r')

F = F_x * Body.x + F_y * Body.y + F_z * Body.z
G = L * Body.x + M * Body.y + N * Body.z
V = u * Body.x + v * Body.y + w * Body.z
W = p * Body.x + q * Body.y + r * Body.z

F = (V.dt(Body)) + (W ^ V)
F

G = ((I & W).dt(Body)) + (W ^ (I & W))
G

Body.ang_vel_in(Hor)

ang_vel = Body.ang_vel_in(Hor)-(p * Body.x + q * Body.y + r * Body.z)
solve([ang_vel.args[0][0][i] for i in range(3)],[diff(phi),diff(theta),diff(psi)])