import numpy as np
import matplotlib.pyplot as plt
plt.interactive(True)

''' 
	function returns the drag forces 
    Fp, Fi and Fd = Fp+Fi, in units of kN, 
    for an airplane flying level at speed v = CAS, 
    having known Ap=equivalent perp. plate area, 
    and wing loading ratio W/L (L=wingspan,W= airplane weight). 
'''
def forces(v,Ap,W_L):
    rho = 1.225 
    Fp = Ap*(rho/2)*v*v
    Oswald_eps = 0.85
    Fi = 2/(Oswald_eps*np.pi*rho)*(W_L/v)**2
    return(Fi,Fp,Fp+Fi)

'''
    Generate synthetic data with noise
'''
def generate_v_F(Ap,W_L):
    v = np.array([75.,81,86,88,96,99,100,104,\
        105,112,117,120,125,134,146,150,151,160,163])
    Fi,Fp,Ft = forces(v,Ap,W_L)
    rnd1 = np.random.randn(len(v))  # sigma = 1
    rnd2 = np.random.randn(len(v))  # sigma = 1
    Ft = Ft*(1+0.01*rnd1) + 0.012*Ft[10]*rnd2
    return(v,Ft)

                 
'''    
    Recover by Least Squares 
    (chi2->min) method two aircraft parameters
    Ap and C. Everything's in S.I. units.
    Fit is in the form:
    F(v) = C * F0(v) + Ap * F1(v)
    F0 = 1/v**2, F1 = (rho/2)*v^2, rho = 1.225
'''
def find_params (vdata,Fdata):
    A = np.zeros((2,2),dtype=float)
    b = np.zeros((2),dtype=float)    
# fill A and b
    rho_2 = 1.225/2       # standard air density [kg/m^3]
    F0 = vdata**(-2)      # induced drag for unit coeff C
    F1 = rho_2 *vdata**2  # drag force for unit plate area
    b[0] = np.sum(F0 * Fdata) 							# create 
    b[1] = np.sum(F1 * Fdata) 	
# fill array A[i,j]
    cross  = np.sum(F0*F1)
    A[0,0] = np.sum(F0*F0)
    A[0,1] = cross
    A[1,0] = cross 
    A[1,1] = np.sum(F1*F1)
# solve 2x2 matrix equation   A p = b, where p = [C,Ap]
    par = np.linalg.solve(A,b)
# least squares fit parameters
    C = par[0]
    Ap = par[1]
    return(C,Ap)


'''
	Main program fit-drag-1.py

    Generate noisy data and store in a file.
    
    Fit airplane drag model model (N=2 parameters) 
	to experimental data about drag force vs. speed. 
    Total drag force is a sum of:
	(i) induced drag force Fi = 2/pi rho^-1 g^2 (M_L/v)**2
        simplified as Fi = C/v**2
	(ii) parasitic drag force Fp = Ap (rho v^2/2) 
    F = C v**(-2)  + Ap (rho v^2/2)
    Two unknowns: C, Ap fit by Least Squares method to data. 
    
    From the fit the program calculates the best speed 
    for: minimum fuel consumption enroute, minimum glide angle
    without engines, minimum (time of travel enroute * used fuel)
'''

plt.figure(dpi=140)
Area, M, L, Cdp = 200, 90e3, 37.7, 0.03
g, rho = 9.81, 1.225 
W_L = g*M/L
Ap = Cdp*Area
print(' assumed A,M,L,',Area,M,L,'\n Ap, W_L',Ap,W_L)
vdata, Fdata = generate_v_F(Ap,W_L)
print(' v data',vdata) 
print(' generated Fdata [T]',Fdata/9.81e3)   
C_hidden = 2/rho/np.pi*W_L**2/0.85    # 0.85 is Oswald epsilon
Ap_hidden = Ap

# plot
v = np.linspace(65,170,200)
F_hidden = C_hidden/(v*v) + Ap_hidden * rho/2 *v*v    
plt.scatter(vdata,Fdata/9.81e3)
plt.plot(v,F_hidden/9.81e3)
plt.grid()
plt.xlabel('V$_{IAS}$[m/s]')
plt.ylabel('drag force  F[T]')
plt.show()
input('fit? ')    
                 
# find parameters C and Ap by Least Squares
                 
C, Ap = find_params(vdata,Fdata)                 
print(' obtained parameters: ',C,Ap)
print(' obtained ratios    : ',C/C_hidden,Ap/Ap_hidden)
# best fit for exerimental v's   
F_fit = C /vdata**2 + Ap * (rho/2) *vdata**2
# best fit for 200 v's from 60 to 170 m/s
F = C/(v*v) + Ap * rho/2 *v*v         
plt.plot(v,F/9.81e3,color=(1,0,0))
plt.scatter(vdata,F_fit/9.81e3,s=9,color=(1,0,0)) 
plt.title('2-param. model of drag vs. speed of jet airliner')
# calculate best speed
# dF/dv = -2C/v**3 + Ap rho v == 0 ==> v_fuel = v_glideslope
# d(F*v)/dv = -C/v**2 + 1.5*Ap*rho*v**2 == 0 ==> v_power = v_T_glide
# d(F/v)/dv = -3C/v**4 +Ap rho/2 ==0 ==> v{min_fuel*time}
v_glidesl = v_fuel =(2*C/Ap)**(1/4)     # in [m/s]
v_T_glide = v_power = (C/Ap)**(1/4) 
v_T_fuel       =    (6*C/Ap)**(1/4) 
print(' best speeds:')
print(' v_T_glide = v_min_pow = ',v_power,int(v_power*3.6))
print(' v_glideslope = v_fuel = ',v_fuel,int(v_fuel*3.6))
print(' v_T_fuel   = v_Carson = ',v_T_fuel,int(v_T_fuel*3.6))
plt.plot([v_fuel,v_fuel],[11.2,7.2])
plt.text(v_fuel,11.1,' v_fuel = '+str(int(v_fuel*3.6))+' km/h IAS')
plt.plot([v_power,v_power],[11.9,7.5])
plt.text(v_power,11.8,' v_power = '+str(int(v_power*3.6))+' km/h IAS')
plt.plot([v_T_fuel,v_T_fuel],[10,8.8])
plt.text(104,10.1,' v_T_fuel = '+str(int(v_T_fuel*3.6))+' km/h IAS')
plt.show()
input(' ok?')
tab =np.array([vdata,Fdata])
np.savetxt("drag_test.dat",tab.T,delimiter=" ")