%% Parse File
% This block wil open a dialog box to permit you to open a .TXT file from
% a Teensy Data Logger to process. It's probably best if you've trimmed the
% file down to just the flight data plus a little pre- and post-flight data
% for calculating baselines.
[fname, pname] = uigetfile('*.TXT','Select file containing Teensy IMU Data strings');
fileID = fopen([pname,fname],'r');
A = textscan(fileID,'%s','Delimiter','\n'); % Read in the file as a set of
                                            % lines of strings 
len = size(A{1,1});
len(1) % len(1) is the number of lines in the file.
i=1; j=1; k=1; l=1; m=1; n=1; p=1;
% Parse each line in the file after the descriptive header to assign the
% values. MATLAB keeps claiming that this process can be speeded up by
% pre-allocating the arrays, but after spending some time to do so, I could
% find no -pre-allocation that actually speeded up anything.
for i = 7:len(1)-7
    switch  A{1,1}{i}(1:3)
        case 'ICM'
            Accel{j} = textscan(A{1,1}{i}, '%s %s %f %s %f %s %f %s %f %s %f %s %f %s %f %s %f %s %f %s %f %s %f','Delimiter',',');
            j = j + 1;
        case 'LIS'
            AccelHG{k}=textscan(A{1,1}{i}, '%s %s %f %s %f %s %f %s %f','Delimiter',',');
            k = k + 1;
        case 'MS5'
            Alt{l} = textscan(A{1,1}{i}, '%s %s %f %s %f %s %f','Delimiter',',');
            l = l + 1;
        case 'ADC'
            ADC{m} = textscan(A{1,1}{i}, '%s %s %f %s %f %s %f','Delimiter',',');
            m = m + 1;
        case 'VOL'
            Voltage{n} = textscan(A{1,1}{i}, '%s %s %f %s %f','Delimiter',',');
            n = n + 1;
        case 'GPS'
            GPS{p} = textscan(A{1,1}{i}, '%s %s %f %s %f %s %f %s %f %s %f %s %f %s %s','Delimiter',',');
            p = p + 1;
        otherwise
            'Bad Line Read'
    end
end
%% Separate Arrays
% Each of the outputs is separated into individual arrays for processing
for index = 1:j-1
    AccelTime(index) = Accel{index}{3};
    Accelx(index) = Accel{index}{5};
    Accely(index) = Accel{index}{7};
    Accelz(index) = Accel{index}{9};
    Gyrox(index) = Accel{index}{11};
    Gyroy(index) = Accel{index}{13};
    Gyroz(index) = Accel{index}{15};
    Magx(index) = Accel{index}{17};
    Magy(index) = Accel{index}{19};
    Magz(index) = Accel{index}{21};
    Taccel(index) = Accel{index}{23};
end
AccelTime = AccelTime/1e6; % The times in the log files are in microseconds.
for index = 1:k-1
    HGTime(index) = AccelHG{index}{3};
    HGx(index) = AccelHG{index}{5};
    HGy(index) = AccelHG{index}{7};
    HGz(index) = AccelHG{index}{9};
end
HGTime = HGTime/1e6; % The times in the log files are in microseconds.
for index = 1:l-1
    AltTime(index) = Alt{index}{3};
    Talt(index) = Alt{index}{5};
    Press(index) = Alt{index}{7};
end
AltTime = AltTime/1e6; % The times in the log files are in microseconds.
for index = 1:m-1
    ADCTime(index) = ADC{index}{3};
    TADC1(index) = ADC{index}{5};
    TADC2(index) = ADC{index}{7};
end
ADCTime = ADCTime/1e6; % The times in the log files are in microseconds.
for index = 1:n-1
    VTime(index) = Voltage{index}{3};
    Volt(index) = Voltage{index}{5};
end
VTime = VTime/1e6; % The times in the log files are in microseconds.
for index = 1:p-1
    GTime(index) = GPS{index}{3};
    Latitude(index) = GPS{index}{5}*1e-7;
    Longitude(index) = GPS{index}{7}*1e-7;
    AltGPS(index) = GPS{index}{9}*1e-3;
    AltErrGPS(index) = GPS{index}{11}*1e-3;
    SIV(index) = GPS{index}{13};
    DT(index) = datetime(GPS{index}{15},'InputFormat','yyyy-MM-dd-HH:mm:ss.S');
end
if p > 1
    GTime = GTime/1e6; % The times in the log files are in microseconds.
end

%% Plot Raw Data
% Make sure all of the data are what you're expecting and that things have
% parsed correctly.
figure(1)
clf('reset')
plot(AccelTime,Accelx,AccelTime,Accely,AccelTime,Accelz)
title('Accelerometer Data')
xlabel('time (s)')
ylabel('Acceleration (milli-g)')
legend('a_z', 'a_x', 'a_y')
figure(2)
clf('reset')
plot(AccelTime,Gyrox,AccelTime,Gyroy,AccelTime,Gyroz)
title('Rotation Rate Data ')
xlabel('time (s)')
ylabel('Rotation Rate (Degrees/s)')
legend('\omega_z', '\omega_x', '\omega_y')
figure(3)
clf('reset')
plot(AccelTime,Magx,AccelTime,Magy,AccelTime,Magz)
title('Raw Magnetometer Data')
xlabel('time (s)')
ylabel('Magnetic Field Strength')
legend('m_x', 'm_y', 'm_z')
figure(4)
clf('reset')
plot(HGTime,HGx,HGTime,HGy,HGTime,HGz)
title('High-G Accelerometer Data')
xlabel('time (s)')
ylabel('Acceleration (g)')
legend('a_z', 'a_x', 'a_y')
figure(5)
clf('reset')
plot(AltTime,Press)
title('Altimeter Data')
xlabel('time (s)')
ylabel('Pressure (Pa)')
figure(6)
clf('reset')
plot(ADCTime,TADC1,ADCTime,TADC2,AccelTime,Taccel,AltTime,Talt)
title('AD Data')
xlabel('time (s)')
ylabel('Temperature (°C)')
legend('TADC1', 'TADC', 'Accel', 'Alt')
figure(7)
clf('reset')
plot(VTime,Volt)
title('Battery Voltage')
xlabel('time (s)')
ylabel('Battery Voltage (V)')
if p > 1
    figure(8)
    clf('reset')
    plot(GTime,AltGPS, GTime, AltGPS+AltErrGPS, GTime, AltGPS-AltErrGPS)
    title('GPS Altitude with Error')
    xlabel('time (s)')
    ylabel('GPS Altitude (m)')
    legend('Alt', '+err', '-err')
    figure(9)
    clf('reset')
    plot(GTime,Latitude,GTime,Longitude)
    title('GPS Latitude, Longitude')
    xlabel('time (s)')
    ylabel('Latitude, Longitude (deg)')
    legend('Lat', 'Lon')
    figure(10)
    clf('reset')
    plot(GTime,SIV)
    title('GPS Satellites in View')
    xlabel('time (s)')
    ylabel('Number of Satellites')
end
%% Rescale Accelerometers to m/s^2
% The ICM data are in milli-g's and the LIS data are in g's. For processing
% and integrating the data should be in SI (m/s^). It wouldn't hurt to
% convert the rotation rates to radians per second.
Accelx = 0.00980665*Accelx;
Accely = 0.00980665*Accely;
Accelz = 0.00980665*Accelz;
HGx = 9.80665*HGx;
HGy = 9.80665*HGy;
HGz = 9.80665*HGz;
%% Resample HG to correspond with Accel
% The ICM and LIS sample at slightly different rates. Since we have to
% combine the two sets of accelerometer data at some point, we have to
% choose what to do. We could:
% 1. Resample the LIS data at the ICM points.
% 2. Resample the ICM data at the LIS points.
% 3. Establish a new time array, such as every millisecond and resample
% both the LIS and ICM to the new time array. I've currently chosen 1.
HGyrs = interp1(HGTime,HGy,AccelTime,'pchip'); % pchip gives the best resampling for real data.
HGxrs = interp1(HGTime,HGx,AccelTime,'pchip');
HGzrs = interp1(HGTime,HGz,AccelTime,'pchip');
%% Set Zero and End
% For further processing you need to divide up the data into pre-launch,
% from launch to apogee (or ejection), descent, and post-landing portions.
% Zoom in on the plots to determine the correct values for the transition
% times.
t0 = 471.153; % Set the starting value for the flight by zooming in on the plots
ta = 482.525; % Set the apogee time
tf = 619.642; % Set the landing time
tPreLen = 20.; % How much time to read the pre-launch data
tpr = t0 - tPreLen;
tPostLen = 30.; % How much time to read the post-launch data
tpo = tf + tPostLen; %
apogeeTime = ta - t0; % Length for liftoff to apogee

%% Get Accelerometer Pre, Flight, and Post data
% Breaks up the time array into the flight portions described above.
it0 = find(AccelTime >= t0,1);
ita = find(AccelTime >= ta,1);
itf = find(AccelTime >= tf,1);
itpr = find(AccelTime >= tpr,1);
itpo = find(AccelTime >= tpo,1);

AccelTimePre = AccelTime(itpr:it0);
AccelxPre = Accelx(itpr:it0);
AccelyPre = Accely(itpr:it0);
AccelzPre = Accelz(itpr:it0);

% Do also for High-g so we can remove baseline errors from High-g
HGxPre = HGxrs(itpr:it0);
HGyPre = HGyrs(itpr:it0);
HGzPre = HGzrs(itpr:it0);

% Get baselines for Accel & High g and Adjust HG to correspond with Accel.
% The LIS has substantially more noise that the ICM and the zero offsets
% appear to be much worse. We reset the baselines for the LIS data to
% correspond to the ICM baselines so that we can combine them.
mAyP = mean(AccelyPre);
mAxP = mean(AccelxPre);
mAzP = mean(AccelzPre);
mHyP = mean(HGyPre);
mHxP = mean(HGxPre);
mHzP = mean(HGzPre);
HGyf = HGyrs-mHyP+mAyP; % Subtract the LIS baseline and add the ICM baseline.
HGxf = HGxrs-mHxP+mAxP; % Subtract the LIS baseline and add the ICM baseline.
HGzf = HGzrs-mHzP+mAzP; % Subtract the LIS baseline and add the ICM baseline.

% This next set of lines creates new acceleration arrays that use the ICM
% accelerometers if they are in range and the LIS accelerometers for values
% outside the range of the ICM accelerometers. Since we converted
% everything to m/s^2 we use 156.5 m/s^2 rather than 15.96 g as the switch
% point.
Logical = abs(Accelx)<156.5; % Switch if Accel above 15.96 g
Accelxf = Accelx.*Logical + HGxf.*(1-Logical);
Logical = abs(Accely)<156.5; % Switch if Accel above 15.96 g
Accelyf = Accely.*Logical + HGyf.*(1-Logical);
Logical = abs(Accelx)<156.5; % Switch if Accel above 15.96 g
Accelzf = Accelz.*Logical + HGzf.*(1-Logical);

AccelTimeFlight = AccelTime(it0:itf);
AccelxFlight = Accelxf(it0:itf);
AccelyFlight = Accelyf(it0:itf);
AccelzFlight = Accelzf(it0:itf);

AccelTimeBoost = AccelTime(it0:ita);
AccelxBoost = Accelxf(it0:ita);
AccelyBoost = Accelyf(it0:ita);
AccelzBoost = Accelzf(it0:ita);

AccelTimeDesc = AccelTime(ita:itf);
AccelxDesc = Accelxf(ita:itf);
AccelyDesc = Accelyf(ita:itf);
AccelzDesc = Accelzf(ita:itf);

AccelTimePost = AccelTime(itf:itpo);
AccelxPost = Accelxf(itf:itpo);
AccelyPost = Accelyf(itf:itpo);
AccelzPost = Accelzf(itf:itpo);

% Recenter time arrays to be based around liftoff. Setting t = 0 to be the
% liftoff time is a time honored rocketry tradition.
AccelTimePre = AccelTimePre - t0;
AccelTimeFlight = AccelTimeFlight - t0;
AccelTimeBoost = AccelTimeBoost - t0;
AccelTimeDesc = AccelTimeDesc - t0;
AccelTimePost = AccelTimePost - t0;

%% Do the Combined Accelerometer plots.
% Plot and make sure the combining of data and the separation into flight
% events makes sense.
% Plot pre data
figure(1)
plot(AccelTimePre,AccelxPre,AccelTimePre,AccelyPre,AccelTimePre,AccelzPre)
title('Accelerometer Data Pre')
xlabel('time (s)')
ylabel('Acceleration (m/s^2)')
legend('a_z', 'a_x', 'a_y')

% Plot flight data
figure(2)
plot(AccelTimeFlight,AccelxFlight,AccelTimeFlight,AccelyFlight,AccelTimeFlight,AccelzFlight)
title('Accelerometer Data Flight')
xlabel('time (s)')
ylabel('Acceleration (m/s^2)')
legend('a_z', 'a_x', 'a_y')

% Plot boost (from liftoff to apogee) data
figure(3)

plot(AccelTimeBoost,AccelxBoost,AccelTimeBoost,AccelyBoost,AccelTimeBoost,AccelzBoost)
title('Accelerometer Data Boost')
xlabel('time (s)')
ylabel('Acceleration (m/s^2)')
legend('a_z', 'a_x', 'a_y')

% Plot 
figure(4)
plot(AccelTimeDesc,AccelxDesc,AccelTimeDesc,AccelyDesc,AccelTimeDesc,AccelzDesc)
title('Accelerometer Data Descent')
xlabel('time (s)')
ylabel('Acceleration (m/s^2)')
legend('a_z', 'a_x', 'a_y')

figure(5)
clf('reset')
plot(AccelTimePost,AccelxPost,AccelTimePost,AccelyPost,AccelTimePost,AccelzPost)
title('Accelerometer Data Post')
xlabel('time (s)')
ylabel('Acceleration (m/s^2)')
legend('a_z', 'a_x', 'a_y')

%% Get Gyro Pre, Flight, and Post data
% DO the same thing for the rate gyro data.
it0 = find(AccelTime >= t0,1);
ita = find(AccelTime >= ta,1);
itf = find(AccelTime >= tf,1);
itpr = find(AccelTime >= tpr,1);
itpo = find(AccelTime >= tpo,1);

GyroTimePre = AccelTime(itpr:it0);
GyroxPre = Gyrox(itpr:it0);
GyroyPre = Gyroy(itpr:it0);
GyrozPre = Gyroz(itpr:it0);

GyroTimeFlight = AccelTime(it0:itf);
GyroxFlight = Gyrox(it0:itf);
GyroyFlight = Gyroy(it0:itf);
GyrozFlight = Gyroz(it0:itf);

GyroTimeBoost = AccelTime(it0:ita);
GyroxBoost = Gyrox(it0:ita);
GyroyBoost = Gyroy(it0:ita);
GyrozBoost = Gyroz(it0:ita);

GyroTimeDesc = AccelTime(ita:itf);
GyroxDesc = Gyrox(ita:itf);
GyroyDesc = Gyroy(ita:itf);
GyrozDesc = Gyroz(ita:itf);

GyroTimePost = AccelTime(itf:itpo);
GyroxPost = Gyrox(itf:itpo);
GyroyPost = Gyroy(itf:itpo);
GyrozPost = Gyroz(itf:itpo);

% Recenter time arrays to be based around liftoff
GyroTimePre = GyroTimePre - t0;
GyroTimeFlight = GyroTimeFlight - t0;
GyroTimeBoost = GyroTimeBoost - t0;
GyroTimeDesc = GyroTimeDesc - t0;
GyroTimePost = GyroTimePost - t0;

% Plot pre data
figure(1)
plot(GyroTimePre,GyroxPre,GyroTimePre,GyroyPre,GyroTimePre,GyrozPre)
title('Gyro Data Pre')
xlabel('time (s)')
ylabel('Rotation Rate (°/s)')
legend('\omega_x', '\omega_y', '\omega_z')

% Plot flight data
figure(2)
plot(GyroTimeFlight,GyroxFlight,GyroTimeFlight,GyroyFlight,GyroTimeFlight,GyrozFlight)
title('Gyro Data Flight')
xlabel('time (s)')
ylabel('Rotation Rate (°/s)')
legend('\omega_x', '\omega_y', '\omega_z')

% Plot boost data
figure(3)
plot(GyroTimeBoost,GyroxBoost,GyroTimeBoost,GyroyBoost,GyroTimeBoost,GyrozBoost)
title('Gyro Data Boost')
xlabel('time (s)')
ylabel('Rotation Rate (°/s)')
legend('\omega_x', '\omega_y', '\omega_z')

% Plot 
figure(4)
plot(GyroTimeDesc,GyroxDesc,GyroTimeDesc,GyroyDesc,GyroTimeDesc,GyrozDesc)
title('Gyro Data Descent')
xlabel('time (s)')
ylabel('Rotation Rate (°/s)')
legend('\omega_x', '\omega_y', '\omega_z')

figure(5)
clf('reset')
plot(GyroTimePost,GyroxPost,GyroTimePost,GyroyPost,GyroTimePost,GyrozPost)
title('Gyro Data Post')
xlabel('time (s)')
ylabel('Rotation Rate (°/s)')
legend('\omega_x', '\omega_y', '\omega_z')

%% Get Magnetometer Pre, Flight, and Post data
% Do the same thing for the magnetometer data.
it0 = find(AccelTime >= t0,1);
ita = find(AccelTime >= ta,1);
itf = find(AccelTime >= tf,1);
itpr = find(AccelTime >= tpr,1);
itpo = find(AccelTime >= tpo,1);

MagTimePre = AccelTime(itpr:it0);
MagxPre = Magx(itpr:it0);
MagyPre = Magy(itpr:it0);
MagzPre = Magz(itpr:it0);

MagTimeFlight = AccelTime(it0:itf);
MagxFlight = Magx(it0:itf);
MagyFlight = Magy(it0:itf);
MagzFlight = Magz(it0:itf);

MagTimeBoost = AccelTime(it0:ita);
MagxBoost = Magx(it0:ita);
MagyBoost = Magy(it0:ita);
MagzBoost = Magz(it0:ita);

MagTimeDesc = AccelTime(ita:itf);
MagxDesc = Magx(ita:itf);
MagyDesc = Magy(ita:itf);
MagzDesc = Magz(ita:itf);

MagTimePost = AccelTime(itf:itpo);
MagxPost = Magx(itf:itpo);
MagyPost = Magy(itf:itpo);
MagzPost = Magz(itf:itpo);

% Recenter time arrays to be based around liftoff
MagTimePre = MagTimePre - t0;
MagTimeFlight = MagTimeFlight - t0;
MagTimeBoost = MagTimeBoost - t0;
MagTimeDesc = MagTimeDesc - t0;
MagTimePost = MagTimePost - t0;

% Plot pre data
figure(1)
plot(MagTimePre,MagxPre,MagTimePre,MagyPre,MagTimePre,MagzPre)
title('Magnetometer Data Pre')
xlabel('time (s)')
ylabel('Magnetic Field Strength')
legend('m_x', 'm_y', 'm_z')

% Plot flight data
figure(2)
plot(MagTimeFlight,MagxFlight,MagTimeFlight,MagyFlight,MagTimeFlight,MagzFlight)
title('Magnetometer Data Flight')
xlabel('time (s)')
ylabel('Magnetic Field Strength')
legend('m_x', 'm_y', 'm_z')

% Plot boost data
figure(3)
plot(MagTimeBoost,MagxBoost,MagTimeBoost,MagyBoost,MagTimeBoost,MagzBoost)
title('Magnetometer Data Boost')
xlabel('time (s)')
ylabel('Magnetic Field Strength')
legend('m_x', 'm_y', 'm_z')

% Plot 
figure(4)
plot(MagTimeDesc,MagxDesc,MagTimeDesc,MagyDesc,MagTimeDesc,MagzDesc)
title('Magnetometer Data Descent')
xlabel('time (s)')
ylabel('Magnetic Field Strength')
legend('m_x', 'm_y', 'm_z')

figure(5)
clf('reset')
plot(MagTimePost,MagxPost,MagTimePost,MagyPost,MagTimePost,MagzPost)
title('Magnetometer Data Post')
xlabel('time (s)')
ylabel('Magnetic Field Strength')
legend('m_x', 'm_y', 'm_z')

%% Get High G Accelerometer Pre, Flight, and Post data
% We don't actually use the LIS data by themselves, and we've already
% combined them with the ICM data, so this section could be deleted if
% desired.
it0 = find(AccelTime >= t0,1);
ita = find(AccelTime >= ta,1);
itf = find(AccelTime >= tf,1);
itpr = find(AccelTime >= tpr,1);
itpo = find(AccelTime >= tpo,1);

HGTimePre = AccelTime(itpr:it0);
HGxPre = HGxf(itpr:it0);
HGyPre = HGyf(itpr:it0);
HGzPre = HGzf(itpr:it0);

HGTimeFlight = AccelTime(it0:itf);
HGxFlight = HGxf(it0:itf);
HGyFlight = HGyf(it0:itf);
HGzFlight = HGzf(it0:itf);

HGTimeBoost = AccelTime(it0:ita);
HGxBoost = HGxf(it0:ita);
HGyBoost = HGyf(it0:ita);
HGzBoost = HGzf(it0:ita);

HGTimeDesc = AccelTime(ita:itf);
HGxDesc = HGxf(ita:itf);
HGyDesc = HGyf(ita:itf);
HGzDesc = HGzf(ita:itf);

HGTimePost = AccelTime(itf:itpo);
HGxPost = HGxf(itf:itpo);
HGyPost = HGyf(itf:itpo);
HGzPost = HGzf(itf:itpo);

% Recenter time arrays to be based around liftoff
HGTimePre = HGTimePre - t0;
HGTimeFlight = HGTimeFlight - t0;
HGTimeBoost = HGTimeBoost - t0;
HGTimeDesc = HGTimeDesc - t0;
HGTimePost = HGTimePost - t0;

%% Get Altimeter Pre, Flight, and Post data
% We will now take the altimeter pressure data, process them to calculate
% the corrected barometric altitudes, process them into the various flight
% segments and plot them.
%
% Set altimeter parameters. Use those at the launch site if you have them.
T0 = 277.59; % Reference temperature in K. Std Atm is 288.15
% P0 is calculated from the pre-launch data. Uncomment the next line and
% comment out the P0 calc if you want to use a different value for P0.
% P0 = 92300; % Reference pressure in Pa. Std Atm is 101325
h0 = 869; % Reference altitude in m. Std Atm is 0.
R = 8.31432; % The molar gas constant. Std Atm is 8.31432 J/mol K
Hr = 0.56; % Relative humidity as fraction. Std Atm is 0.
Mair = 0.0289644; % Molecular weight of air in kg/mol. Std Atm is 0.0289644
MH2O = 0.01801528; % Molecular weight of water in kg/mol.
L = -0.0065; % Temperature lapse rate. Std Atm is -0.0065 K/m below 11 km
g = 9.80665; % Standard acceleration of gravity in m/s^2
PH2O = exp(16.3872-3885.70/(T0-42.98))*1000*Hr; % Water vapor pressure Antoine equation

it0 = find(AltTime >= t0,1);
ita = find(AltTime >= ta,1);
itf = find(AltTime >= tf,1);
itpr = find(AltTime >= tpr,1);
itpo = find(AltTime >= tpo,1);

AltTimePre = AltTime(itpr:it0);
PressPre = Press(itpr:it0);
P0 = mean(PressPre); % Use instead of setting P0 above.
M = (MH2O*PH2O+Mair*(P0-PH2O))/P0;
MSL = h0+T0/L*((Press/P0).^(-(L*R/M/g))-1); % The MSL Altitude as calculated by the Standard Atmosphere.
MSLPre = MSL(itpr:it0);

AltTimeFlight = AltTime(it0:itf);
PressFlight = Press(it0:itf);
MSLFlight = MSL(it0:itf);

AltTimeBoost = AltTime(it0:ita);
PressBoost = Press(it0:ita);
MSLBoost = MSL(it0:ita);

AltTimeDesc = AltTime(ita:itf);
PressDesc = Press(ita:itf);
MSLDesc = MSL(ita:itf);

AltTimePost = AltTime(itf:itpo);
PressPost = Press(itf:itpo);
MSLPost = MSL(itf:itpo);

% Recenter time arrays to be based around liftoff
AltTimePre = AltTimePre - t0;
AltTimeFlight = AltTimeFlight - t0;
AltTimeBoost = AltTimeBoost - t0;
AltTimeDesc = AltTimeDesc - t0;
AltTimePost = AltTimePost - t0;

% Plot pre data
figure(1)
yyaxis right
plot(AltTimePre,PressPre)
title('Altimeter Data Prelaunch')
xlabel('time (s)')
ylabel('Pressure (Pa)')
yyaxis left
plot(AltTimePre,MSLPre)
ylabel('Altitude MSL (m)')
legend('MSL', 'P')


% Plot flight data
figure(2)
yyaxis right
plot(AltTimeFlight,PressFlight)
title('Altimeter Data Flight')
xlabel('time (s)')
ylabel('Pressure (Pa)')
yyaxis left
plot(AltTimeFlight,MSLFlight)
ylabel('Altitude MSL (m)')
legend('MSL', 'P')

% Plot boost data
figure(3)
yyaxis right
plot(AltTimeBoost,PressBoost)
title('Altimeter Data Boost')
xlabel('time (s)')
ylabel('Pressure (Pa)')
yyaxis left
plot(AltTimeBoost,MSLBoost)
ylabel('Altitude MSL (m)')
legend('MSL', 'P')

% Plot 
figure(4)
yyaxis right
plot(AltTimeDesc,PressDesc)
title('Altimeter Data Descent')
xlabel('time (s)')
ylabel('Pressure (Pa)')
yyaxis left
plot(AltTimeDesc,MSLDesc)
ylabel('Altitude MSL (m)')
legend('MSL', 'P')

figure(5)
yyaxis right
plot(AltTimePost,PressPost)
title('Altimeter Data Post-flight')
xlabel('time (s)')
ylabel('Pressure (Pa)')
yyaxis left
plot(AltTimePost,MSLPost)
ylabel('Altitude MSL (m)')
legend('MSL', 'P')

%% Get ADC1, ADC2 Pre, Flight, and Post data
% Process the thermistor data from the ADCs
it0 = find(ADCTime >= t0,1);
ita = find(ADCTime >= ta,1);
itf = find(ADCTime >= tf,1);
itpr = find(ADCTime >= tpr,1);
itpo = find(ADCTime >= tpo,1);

ADCTimePre = ADCTime(itpr:it0);
TADC1Pre = TADC1(itpr:it0);
TADC2Pre = TADC2(itpr:it0);

ADCTimeFlight = ADCTime(it0:itf);
TADC1Flight = TADC1(it0:itf);
TADC2Flight = TADC2(it0:itf);

ADCTimeBoost = ADCTime(it0:ita);
TADC1Boost = TADC1(it0:ita);
TADC2Boost = TADC2(it0:ita);

ADCTimeDesc = ADCTime(ita:itf);
TADC1Desc = TADC1(ita:itf);
TADC2Desc = TADC2(ita:itf);

ADCTimePost = ADCTime(itf:itpo);
TADC1Post = TADC1(itf:itpo);
TADC2Post = TADC2(itf:itpo);

% Recenter time arrays to be based around liftoff
ADCTimePre = ADCTimePre - t0;
ADCTimeFlight = ADCTimeFlight - t0;
ADCTimeBoost = ADCTimeBoost - t0;
ADCTimeDesc = ADCTimeDesc - t0;
ADCTimePost = ADCTimePost - t0;

% Plot pre data
figure(1)
clf('reset')
plot(ADCTimePre,TADC1Pre,ADCTimePre,TADC2Pre)
title('AD Data Prelaunch')
xlabel('time (s)')
ylabel('Temperature (°C)')
legend('TADC1', 'TADC')

% Plot flight data
figure(2)
clf('reset')
plot(ADCTimeFlight,TADC1Flight,ADCTimeFlight,TADC2Flight)
title('AD Data Flight')
xlabel('time (s)')
ylabel('Temperature (°C)')
legend('TADC1', 'TADC2')

% Plot boost data
figure(3)
clf('reset')
plot(ADCTimeBoost,TADC1Boost,ADCTimeBoost,TADC2Boost)
title('AD Data Boost')
xlabel('time (s)')
ylabel('Temperature (°C)')
legend('TADC1', 'TADC2')

% Plot 
figure(4)
clf('reset')
plot(ADCTimeDesc,TADC1Desc,ADCTimeDesc,TADC2Desc)
title('AD Data Descent')
xlabel('time (s)')
ylabel('Temperature (°C)')
legend('TADC1', 'TADC2')

figure(5)
clf('reset')
plot(ADCTimePost,TADC1Post,ADCTimePost,TADC2Post)
title('AD Data Post Flight')
xlabel('time (s)')
ylabel('Temperature (°C)')
legend('TADC1', 'TADC2')

% For everything from here on, we are making assumptions about how we want
% to process the data and how the rocket flew. We are assuming that the
% rocket was vertical on the launch pad, and that it flew essentially
% straight up. We're doing a 1-D flight data analysis.

%% Correct accel for 1-g offset
% We assume that the ICM accelerometer is accurate and that it is
% measureing the local acceleration of gravity while on the launch pad. We
% subtract the local 1-g to get the true acceleration in the vertical
% direction. The accuracy of vertical flight falls off substantially as we
% near apogee.
AyTrue = AccelyBoost - mAyP;
Vy = cumtrapz(AccelTimeBoost,AyTrue); % Integrate the acceleration to get the velocity.
Py = cumtrapz(AccelTimeBoost,Vy); % Integrate the velocity to get the altitude.
figure(6)
clf('reset')
plot(AccelTimeBoost,AyTrue, AccelTimeBoost,Vy)
title('Merged Acceleration Array and Velocity Array')
xlabel('time (s)')
ylabel('Acceleration (m/s^2), Velocity (m/s) ')
legend('Acceleration', 'Velocity')

%% Plot with Altitude Data
% Plot both the velocity and altitude calcuated from the acceleration with
% the AGL altitude calcuated from the barometric altimeter.
AGLBoost = MSLBoost-h0;
figure(1)
clf('reset')
yyaxis right
plot(AccelTimeBoost,Py, AltTimeBoost, AGLBoost)
title('Vertical Acceleration, Velocity, and Altitude')
xlabel('time (s)')
ylabel('Altitude (m)')
yyaxis left
plot(AccelTimeBoost,AyTrue, AccelTimeBoost,Vy)
ylabel('Acceleration (m/s^2), Velocity (m/s) ')
legend('Acceleration', 'Velocity', 'Alt', 'Alt (P)')

%% Split for CD and Thrust
% We want to determine the average CD from burnout to apogee, and then
% calcuate the CD as a function of velocity. THe first stepis to determine
% from the acceleration or velocity curve when burnout occurs, and entering
% it below.
burnoutTime = 2.00; % Set from graph
accelTC = AyTrue(AccelTimeBoost<burnoutTime);
velTC = Vy(AccelTimeBoost<burnoutTime);
timeTC = AccelTimeBoost(AccelTimeBoost<burnoutTime);
accelCD = AyTrue(AccelTimeBoost>=burnoutTime);
velCD = Vy(AccelTimeBoost>=burnoutTime);
timeCD = AccelTimeBoost(AccelTimeBoost>=burnoutTime);
figure(2)
clf('reset')
plot(timeTC,accelTC, timeTC,velTC)
title('Check Burn Portion of Ascent')
xlabel('time (s)')
ylabel('Accel & Vel')
legend('Accel', 'Vel')

%% Check CD part
% Plot the acceleration and velocity curved from burnout to apogee to make
% sure we have reasonable data for calcualteing CD and later the thrust
% curve.
figure(3)
clf('reset')
plot(timeCD,accelCD, timeCD,velCD)
title('Check Coast Portion of Ascent')
xlabel('time (s)')
ylabel('Accel & Vel')
legend('Accel', 'Vel')

%% Input Rocket Data
% Calculation of the drag coefficient requires a bunch of the rocket
% properties. Enter them below.
% Data are in kg, m, K, Pa, etc.
rocketMass = 0.33355;
casingMass = 0.059;
propellantMass = 0.065;
diameter = 0.042;
Tabs = T0; % 78F Could also get from thermistors before liftoff.
Pabs = P0;
MAM = M; % Can correct for humidity if desired
Rgas = R;
rho = MAM*Pabs/Rgas/Tabs; % Could make dynamic to follow altitude
Ap = 0.25*pi*diameter^2; % Projected area for drag calculation.
localg = 9.80665; % Can correct for true g at launch site.

%% Calculate average CD
yCD = -2*(rocketMass + casingMass)*(accelCD+localg)./Ap./rho;
vsq = velCD.*velCD;
[si, St] = polyfit(vsq,yCD,1);
% Calculate the fit curve and the upper and lower bounds. Set 'predopt'
% to 'observation' for observational bounds or 'curve' for functional
% bounds. You usually can't see the difference between the fit and the
% functional bounds.
[CDfit,StErr] = polyconf(si,vsq,St,'predopt','curve');
CD = si(1)
figure(4)
clf('reset')
plot(vsq,yCD,'.',vsq,CDfit, vsq, CDfit+StErr,'m--', vsq, CDfit-StErr, 'm--')
title('Check Average CD')
xlabel('v^2 (m^2/s^2)')
ylabel('-2m(a+g)/Ap \rho')
legend('Data', 'Fit', 'Upp Bound',  'Low Bound')

%% Calculate CD vs v
% Calculate the CD versus velocity from the data.
CDarr = yCD./vsq;
figure(5)
clf('reset')
plot(velCD,CDarr,'.')
axis([ 0 inf 0 2])
title('Plot CD vs v')
xlabel('v (m/s)')
ylabel('CD')

%% Determine Thrust Curve
% This section assumes initially that the propellant mass varies linearly
% with time during the burn, and then iterates 5 times to correct for the
% variation of thrust and mass loss with time. To check convergence, see
% how the calculated impulse varies from iteration to iteration
m0 = rocketMass+casingMass+propellantMass;
dmdt = propellantMass/burnoutTime;
mass = m0-dmdt*timeTC; %Begin with linear guess
Tx = mass.*(accelTC+localg)+0.5*CD*Ap*rho*velTC.*velTC;
impulse = trapz(timeTC,Tx)
dmassdt = Tx/impulse*propellantMass;
mass = m0 - cumtrapz(timeTC,dmassdt); %Correct mass with results
Tx = mass.*(accelTC+localg)+0.5*CD*Ap*rho*velTC.*velTC; %2nd Iteration
impulse = trapz(timeTC,Tx)
dmassdt = Tx/impulse*propellantMass;
mass = m0 - cumtrapz(timeTC,dmassdt);
Tx = mass.*(accelTC+localg)+0.5*CD*Ap*rho*velTC.*velTC; %3rd Iteration
impulse = trapz(timeTC,Tx)
dmassdt = Tx/impulse*propellantMass;
mass = m0 - cumtrapz(timeTC,dmassdt);
Tx = mass.*(accelTC+localg)+0.5*CD*Ap*rho*velTC.*velTC; %4th Iteration
impulse = trapz(timeTC,Tx)
dmassdt = Tx/impulse*propellantMass;
mass = m0 - cumtrapz(timeTC,dmassdt);
Tx = mass.*(accelTC+localg)+0.5*CD*Ap*rho*velTC.*velTC; %5th Iteration
impulse = trapz(timeTC,Tx)

% Plot the results of the thrust curve and mass loss curve.
figure(6)
clf('reset')
yyaxis left
plot(timeTC,Tx)
title('Thrust Curve and Mass Change')
xlabel('time (s)')
ylabel('Thrust (N)')
yyaxis right
plot(timeTC,mass)
ylabel('Rocket Mass (kg)')

% You can add additional processing here.