from math import sin, cos, radians, degrees, exp, sqrt, fabs

# Define PSAS:
k_p = 2.45
k_v = 3.2
Cl_base = 3.2
fin_area = 1.13e-3
I_0 = 0.1
I_1 = 0.01

def C_L(a, v):
    """Find C_L for a given speed and angle of attack
    
    :param float a: Angle of attack, alpha in degrees
    :param float v: velocity, v in m/s
    """
    
    # math is in radians
    a = radians(a)
    
    # Subsonic case
    def _subsonic():
        sina = sin(a)
        cosa = cos(a)
        cl = k_p*sina*cosa*cosa
        cl += k_v*cosa*sina*sina
        return cl
    
    # Supersonic case
    def _supersonic():
        cl = a*Cl_base
        return cl

    if v <= 265:
        return _subsonic()
    elif v < 330:
        # Intepolate between super and subsonic
        y0 = _subsonic()
        y1 = _supersonic()
        x0 = 265
        x1 = 330
        cl = y0 + (y1-y0)*(v - x0)/(x1-x0)
        return cl
    else:
        return _supersonic()

def C_L_aprox(a, v):

    # Subsonic case
    def _subsonic():
        af = 0.0006
        bf = 0.045
        cl = af*a**2 + bf*a
        return cl
    
    # Supersonic case
    def _supersonic():
        cl = a*Cl_base
        return cl

    if v <= 265:
        return _subsonic()
    elif v < 330:
        # Intepolate between super and subsonic
        y0 = _subsonic()
        y1 = _supersonic()
        x0 = 265
        x1 = 330
        cl = y0 + (y1-y0)*(v - x0)/(x1-x0)
        return cl
    else:
        return _supersonic()

%matplotlib inline
import matplotlib.pyplot as plt
from matplotlib import rcParams
rcParams.update({'font.size': 16})

# range of angle of attack to chart
alpha = [a*0.5 for a in range(31)]
CL = [C_L(a, 120) for a in alpha]
CLaprox = [C_L_aprox(a, 120) for a in alpha]

fig = plt.figure(figsize=(15,9))
ax = fig.add_subplot(111)
plt.title("Subsonic Angle of Attack and Aproximation")
plt.ylabel(r"$C_{L\alpha}$")
plt.xlabel(r"$\alpha$ [${}^0$]")

plt.plot(alpha, CL, label="Full Solution")
plt.plot(alpha, CLaprox, label="Polynomial Aproximation")
plt.legend(loc=2)
ax.grid(color='grey', alpha=0.3, linestyle='dashed', linewidth=0.5)
plt.show()

def estimate_alpha(x, v, aa, t):
    """Return an estimated fin angle of attack for to
    achieve the required angular acceleration.
    
    :param x: Altitude (meters, MSL)
    :param v: Air velocity (m/s)
    :param aa: Angular acceleration to compute alpha for (degrees/s/s)
    :param t: Time (seconds since launch)
    :returns fin angle:
    
    """
    # LV2.3 Constants
    fin_area = 1.13e-3
    fin_arm = 0.085
    I_0 = 0.086
    I_1 = 0.077
    af = 0.0006
    bf = 0.045
    cl_super = 3.2
    
    
    # compute rho
    rho = 1.2250 * exp((-9.80665 * 0.0289644 * x)/(8.31432*288.15))
    
    # compute I
    if t <= 0:
        I = I_0
    elif t < 5.6:
        I = I_0 + (I_1-I_0)*t/5.6
    else:
        I = I_1

        
        
    def _subsonic():
        alpha = sqrt((2*aa*I*af)/(rho*v*v*fin_area*fin_arm) + bf*bf) - bf
        alpha = alpha / (2*af)
        return alpha
    
    def _supersonic():
        alpha =  (aa*I)/(2*rho*v*v*fin_area*fin_arm*cl_super)
        return degrees(alpha)
    
    
    if v <= 265:
        return _subsonic()
    elif v < 330:
        # Intepolate between super and subsonic
        y0 = _subsonic()
        y1 = _supersonic()
        x0 = 265
        x1 = 330
        cl = y0 + (y1-y0)*(v - x0)/(x1-x0)
        return cl
    else:
        return _supersonic()

def lift(a, v, alt):
    """Compute the lift of one fin at an angle of
    attack, velocity, and altitdue
    
    :param float a: Angle of attack in degrees
    :param float v: velocity in m/s
    :param float alt: altitude MSL in meters
    :returns float lift in Newtons:
    
    """
    
    # get density of atmosphere with quick exponential model
    rho = 1.2250 * exp((-9.80665 * 0.0289644 * alt)/(8.31432*288.15))

    l = 0.5*C_L(a, v)*rho*v*v*fin_area
    
    return l

def anglar_accel(a, x, v, t):
    """Compute angular accelation for a single point
    
    :param a: Fin alpha (degrees)
    :param x: altitude (meters, MSL)
    :param v: air velocity (m/s)
    :param t: time since launch (seconds)
    """
    
    fin_arm = 0.085
    I_0 = 0.086
    I_1 = 0.077

    # compute I
    if t <= 0:
        I = I_0
    elif t < 5.6:
        I = I_0 + (I_1-I_0)*t/5.6
    else:
        I = I_1
    
    
    return (4*lift(a, v, x)*fin_arm)/I

worst = 0
for aa in range(10,30,10):
    for v in range(100,350,25):
        for x in range(1000,5000,500):
            for t in range(0,30,5):
                alpha = estimate_alpha(x,v,aa,t)
                aa_real = anglar_accel(alpha, x, v, t)
                error = fabs((aa_real - aa)/aa_real)*100
                if worst < error:
                    worst = error

print "Estimate never off model by more than %0.1f%%" % worst