from sympy.core import Symbol
from sympy.functions import exp, sin, cos, tan
from sympy.solvers import solve

from das.legacy.Analytics.Mathematics import PoissonManifold
from das.utils.keyboard import keyboard

M = PoissonManifold(name='basic',coords=['h','theta','phi','v','gam','psi','lambda_h','lambda_theta','lambda_phi','lambda_v','lambda_gam','lambda_psi'], independent=['t'], verbose=2)

coords = M.coords

h = coords[0]
theta = coords[1]
phi = coords[2]
v = coords[3]
gam = coords[4]
psi = coords[5]
lambda_h = coords[6]
lambda_theta = coords[7]
lambda_phi = coords[8]
lambda_v = coords[9]
lambda_gam = coords[10]
lambda_psi = coords[11]

re = Symbol('re')
Aref = Symbol('Aref')
rho0 = Symbol('rho0')
H = Symbol('H')
alpha = Symbol('alpha')
beta = Symbol('beta')
mass = Symbol('mass')
mu = Symbol('mu')

rho = rho0*exp(-h/H)
Cl  = 1.5658*alpha
Cd  = 1.6537*alpha**2 + 0.0612
D   = 0.5*rho*v**2*Cd*Aref
L   = 0.5*rho*v**2*Cl*Aref
r   = re+h

X = [0 for _ in range(6)]

X[0] = v*sin(gam)
X[1] = v*cos(gam)*cos(psi)/(r*cos(phi))
X[2] = v*cos(gam)*sin(psi)/r
X[3] = -D/mass - mu*sin(gam)/(r**2)
X[4] = L*cos(beta)/(mass*v) - mu/(v*r**2)*cos(gam) + v/r*cos(gam)
X[5] = L*sin(beta)/(mass*cos(gam)*v) - v/r*cos(gam)*cos(psi)*tan(phi)

H = 1 + X[0]*lambda_h + X[1]*lambda_theta + X[2]*lambda_phi + X[3]*lambda_v + X[4]*lambda_gam + X[5]*lambda_psi

dHdu = [H.diff(alpha), H.diff(beta)]

p = solve(dHdu, alpha, beta)

print(p)

keyboard()