import numpy as np
import control as ctrl
from Cit_par import statespacematrix
import scipy.io as sio
import matplotlib.pyplot as plt
# Stationary flight condition
hp0 = 1500 # pressure altitude in the stationary flight condition [m]
V0 = 82.3 # true airspeed in the stationary flight condition [m/sec]
alpha0 = 0.04 # angle of attack in the stationary flight condition [rad]
th0 = 0.08 # pitch angle in the stationary flight condition [rad]
# Aircraft mass
m = 17000 # mass [kg]
A_sym,B_sym,A_asym,B_asym = statespacematrix(hp0,V0,alpha0,th0,m) #Calling state space matrix
#-----------Importing matlab flight data-------------------
mat_contents = sio.loadmat('FTISxprt-20200305_flight3.mat')
#Importing values from the matlab file
elevator_dte = mat_contents['flightdata'][0][0][1][0][0][0] #Deflection of the trim tab elevator in degree
vane_AOA = mat_contents['flightdata'][0][0][0][0][0][0] #Angle of attack in degree
time = mat_contents['flightdata'][0][0][-1][0][0][0] #Time in seconds
pitch_rate = mat_contents['flightdata'][0][0][27][0][0][0] #Pitch rate in deg per seconds
pitch = mat_contents['flightdata'][0][0][22][0][0][0] #Pitch in degrees
deltae = mat_contents['flightdata'][0][0][17][0][0][0] #Deflection of the elevator in degree
deltar = mat_contents['flightdata'][0][0][18][0][0][0] #Deflection of the rudder in degree
Vtrue = mat_contents['flightdata'][0][0][42][0][0][0] #True airspeed in knots
Vtrue = Vtrue * 0.51444444444444444 #True airspeed in m/s
#We have a pulse shaped rudder deflection on t = 2792 [s] for dutch roll
#We have a pulse shaped rudder deflection on t = 2837 [s] for dutch roll with yaw damper
startvalue =27830 #Starts at index 25360
endvalue = 28000 #We want to see ... seconds after start
# Stationary flight condition
hp0 = Pressure_Altitude[startvalue] # pressure altitude in the stationary flight condition [m]
V0 = Vtrue[startvalue] # true airspeed in the stationary flight condition [m/sec]
alpha0 = vane_AOA[startvalue]*(np.pi/180) # angle of attack in the stationary flight condition [rad]
th0 = pitch[startvalue]*(np.pi/180) # pitch angle in the stationary flight condition [rad]
# Aircraft mass
m = 6378.821 # mass [kg]
A_sym,B_sym,A_asym,B_asym = statespacematrix(hp0[0],V0[0],alpha0[0],th0[0],m) #Calling state space matrix
C = np.identity(4)
D = np.array([[0],
[0],
[0],
[0]])
sys = ctrl.ss(A_sym,B_sym,C,D)
xinit = np.array([10*(np.pi/180),0,0,0])
T ,yout = ctrl.initial_response(sys, T=np.arange(0,100,0.1), X0=xinit)
sideslip_sim = yout[0]*(180/np.pi) #from radians to degree
rollangle_sim = yout[1]*(180/np.pi) #from radians to degree
rollrate_sim = yout[2]*(180/np.pi) #from radians/s to degree/s
yawrate_sim = yout[3]*(180/np.pi) #from radians/s to degree/s
import numpy as np
import control as ctrl
import scipy.io as sio
import matplotlib.pyplot as plt
from math import pi, sin, cos
import cmath
from Cit_par import statespacematrix
hp0 = 1500 # pressure altitude in the stationary flight condition [m]
V0 = 0.000000000000000000000000000001 # true airspeed in the stationary flight condition [m/sec]
alpha0 = 0.04 # angle of attack in the stationary flight condition [rad]
th0 = 0.08 # pitch angle in the stationary flight condition [rad]
m = 10000 # Aircraft mass
print(statespacematrix(hp0,V0,alpha0,th0,m))
from Verification import get_eigenvalues, get_coefficients_3x3_matrix,abcd_formula, abc_formula, get_short_period_eigenvalues,get_phugoid_eigenvalues,get_aperiodic_roll_eigenvalue,get_dutch_roll_eigenvalues, get_spiral_motion_eigenvalue, get_dutch_roll_with_aperiodic_roll_eigenvalues
A= np.array([[2,-1,-1,0],
[-1,3,-1,-1],
[-1,-1,3,-1],
[0,-1,-1,2]])
print(get_eigenvalues(A)) #should be 0,2,4 check
#get_coefficients_3x3_matrix(B)
abcd_formula(1, 0, -9, 0) #x^{3}-9x=0 -> -3,03 check
print(abc_formula(1, 2, -8)) #2,4 check