% This m-file expects input files in the following formats.

% 1. A (preferably trimmed) .TXT file from the Teensy Data Logger. It will
% parse the data and create arrays of the sensor outputs. The parsing takes
% a long time.

% 2. An Open Rocket exported flight data file in CSV format. You should export:
% Time in s
% Altitude in m
% Vertical velocity in m/s
% Vertical acceleration in m/s^2
% Drag Coefficient
% DO NOT export any other variables. You will have to edit the output file
% to remove any events like lift-off or apogee. The final format should
% just have a variabile-name row and rows of numbers.

% 3. A CSV file with the GPS time in the first column and the GPS altitude
% (expected to be in feet MSL) in the second column. You can use either
% data from the log file or from the flight-specific file from the
% Featherweight GPS.

% The m-file will pull up a dialog box to let you choose the files. In
% addition, at the appropriate points in the code you will need to enter

%% Parse Teensy Data Logger 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};
    Gyroxds(index) = Accel{index}{11};
    Gyroyds(index) = Accel{index}{13};
    Gyrozds(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 % THis block is unlikely to run.
    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,Gyroxds,AccelTime,Gyroyds,AccelTime,Gyrozds)
title('Rotation Rate Data ')
xlabel('time (s)')
ylabel('Rotation Rate (°/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 % Again, this block is unlikely to run.
    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 and Gyros to rad/s
% 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;
Gyrox = pi/180*Gyroxds;
Gyroy = pi/180*Gyroyds;
Gyroz = pi/180*Gyrozds;

%% 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 = 1294.145; % Set the starting value for the flight by zooming in on the plots
ta = 1316.705; % Set the apogee time
tf = 1486.3; % Set the landing time
tPreLen = 30.; % 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 from apogee to landing
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')

% Plot post landing data
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
% Calculate and remove offset
GyroxBase = mean(Gyrox(AccelTime < t0));
GyroyBase = mean(Gyroy(AccelTime < t0));
GyrozBase = mean(Gyroz(AccelTime < t0));
Gyrox = Gyrox - GyroxBase;
Gyroy = Gyroy - GyroyBase;
Gyroz = Gyroz - GyrozBase;

% Do the flight event separation 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^{-1})')
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^{-1})')
legend('\omega_x', '\omega_y', '\omega_z')

% Plot boost (from liftoff to apogee) data
figure(3)
plot(GyroTimeBoost,GyroxBoost,GyroTimeBoost,GyroyBoost,GyroTimeBoost,GyrozBoost)
title('Gyro Data Boost')
xlabel('time (s)')
ylabel('Rotation Rate (s^{-1})')
legend('\omega_x', '\omega_y', '\omega_z')

% Plot from apogee to landing
figure(4)
plot(GyroTimeDesc,GyroxDesc,GyroTimeDesc,GyroyDesc,GyroTimeDesc,GyrozDesc)
title('Gyro Data Descent')
xlabel('time (s)')
ylabel('Rotation Rate (s^{-1})')
legend('\omega_x', '\omega_y', '\omega_z')

% Plot post landing data
figure(5)
clf('reset')
plot(GyroTimePost,GyroxPost,GyroTimePost,GyroyPost,GyroTimePost,GyrozPost)
title('Gyro Data Post')
xlabel('time (s)')
ylabel('Rotation Rate (s^{-1})')
legend('\omega_x', '\omega_y', '\omega_z')

%% Scale MagData
magXMax = max(Magx);
magXMin = min(Magx);
magYMax = max(Magy);
magYMin = min(Magy);
magZMax = max(Magz);
magZMin = min(Magz);
magXsc = (2*Magx-(magXMax+magXMin))/(magXMax-magXMin);
magYsc = (2*Magy-(magYMax+magYMin))/(magYMax-magYMin);
magZsc = (2*Magz-(magZMax+magZMin))/(magZMax-magZMin);
magMagsc = sqrt(magXsc.^2 + magYsc.^2 + magZsc.^2);
figure(8)
plot(AccelTime,magXsc, AccelTime,magYsc, AccelTime,magZsc, AccelTime,magMagsc)
title('Scaled Magnetometer Data')
xlabel('time (s)')
ylabel('Magnetic Field Strength')
legend('m_x', 'm_y', 'm_z','mag')

%% Renormalize MagData
% The previous graph shows that the normalized magnitude isn't always 1.00
% so we renormalize so that all of the vectors have magnitude 1.
magXsc = magXsc./magMagsc;
magYsc = magYsc./magMagsc;
magZsc = magZsc./magMagsc;
figure(8)
plot(AccelTime,magXsc, AccelTime,magYsc, AccelTime,magZsc)
title('Scaled Renormalized Magnetometer Data')
xlabel('time (s)')
ylabel('Magnetic Field Strength')
legend('m_x', 'm_y', 'm_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 = magXsc(itpr:it0);
MagyPre = magYsc(itpr:it0);
MagzPre = magZsc(itpr:it0);

MagTimeFlight = AccelTime(it0:itf);
MagxFlight = magXsc(it0:itf);
MagyFlight = magYsc(it0:itf);
MagzFlight = magZsc(it0:itf);

MagTimeBoost = AccelTime(it0:ita);
MagxBoost = magXsc(it0:ita);
MagyBoost = magYsc(it0:ita);
MagzBoost = magZsc(it0:ita);

MagTimeDesc = AccelTime(ita:itf);
MagxDesc = magXsc(ita:itf);
MagyDesc = magYsc(ita:itf);
MagzDesc = magZsc(ita:itf);

MagTimePost = AccelTime(itf:itpo);
MagxPost = magXsc(itf:itpo);
MagyPost = magYsc(itf:itpo);
MagzPost = magZsc(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 (from liftoff to apogee) 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 from apogee to landing
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')

% Plot post landing data
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 = 308.15; % 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 (from liftoff to apogee) 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 from apogee to landing
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')

% Plot post landing data
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 (from liftoff to apogee) data
figure(3)
clf('reset')
plot(ADCTimeBoost,TADC1Boost,ADCTimeBoost,TADC2Boost)
title('AD Data Boost')
xlabel('time (s)')
ylabel('Temperature (°C)')
legend('TADC1', 'TADC2')

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

% Plot post landing data
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(1)
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 = 1.141; % 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 = 2.040;
casingMass = 0.375;
propellantMass = 0.500;
diameter = 0.0575;
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.

%% Start on Acceleration, Velocity, Altitude comparison with Open Rocket
% Load OR Table from file
[fname, pname] = uigetfile('*.csv','Select file containing Open Rocket sim data.');
opts = detectImportOptions(fname);
ORFile = readmatrix(fname,opts);
TimeOR = ORFile(1:end,1);
AltOR = ORFile(1:end,2);
VelOR = ORFile(1:end,3);
AccelOR = ORFile(1:end,4);
CDOR = ORFile(1:end,5);

%% Plot Acceleration Data vs Model
TimeORBoost = TimeOR(TimeOR<ta-t0);
AccelORBoost = AccelOR(TimeOR<ta-t0);
figure(1)
clf('reset')
plot(AccelTimeBoost,AccelyBoost, TimeORBoost, AccelORBoost)
title('Measured and Modeled Acceleration')
xlabel('time (s)')
ylabel('Acceleration m/s^2')
legend('IMU', 'OR')

%% Plot Velocity Data vs Model
VelORBoost = VelOR(TimeOR<ta-t0);
figure(2)
clf('reset')
plot(AccelTimeBoost,Vy, TimeORBoost, VelORBoost)
title('Measured and Modeled Velocity')
xlabel('time (s)')
ylabel('Velocity m/s')
legend('IMU', 'OR')

%% Load GPS Log for correcting barometric altitude
[fname, pname] = uigetfile('*.csv','Select file containing GPS data.');
opts = detectImportOptions(fname);
GPSFile = readmatrix(fname,opts);
TimeGPS = GPSFile(1:end,1);
AltFtGPS = GPSFile(1:end,2);
AltGPS = AltFtGPS*0.3048; % Convert GPS from feet to meters.

% Plot GPS to double check and determine correct start time.
figure(3)
clf('reset')
plot(TimeGPS,AltGPS,'*')

%% Set GPS Start time (t=0) & Scale altitude to m
GPS0 = 14558.45; % The t=0 time (lift-off) for the GPS data
TimeShiftGPS = TimeGPS - GPS0;
% Plot
figure(3)
clf('reset')
plot( TimeShiftGPS, AltGPS, '*', AltTimeFlight, MSLFlight)
title('Plot Altitude vs Time for GPS and Uncorrected Altimeter')
xlabel('time (s)')
ylabel('Altitude (m)')
legend('GPS','Altimeter')

%% Set start of descent fit.
TimeGPSDesc0 = 18.5; % Time when the GPS has reacquired lock and the data are accurate
TimeGPSDesc = TimeShiftGPS(TimeShiftGPS>TimeGPSDesc0);
AltGPSDesc = AltGPS(TimeShiftGPS>TimeGPSDesc0);
plot(TimeGPSDesc,AltGPSDesc,'*')

%% Sample MSLFlight at TimeGPSDesc times
altMSLSyncGPS = interp1(AltTimeFlight,MSLFlight,TimeGPSDesc);
altMSLSyncGPS = rmmissing(altMSLSyncGPS);
minLength = min(length(altMSLSyncGPS), length(AltGPSDesc));
altMSLSyncGPS = altMSLSyncGPS(1:minLength);
AltGPSDescFix = AltGPSDesc(1:minLength);

%% Get GPS vs Alt Slope Intercept and Adjust Barometirc Altitude
coeffs = polyfit(altMSLSyncGPS, AltGPSDescFix, 1)
altMSLCorr = coeffs(1)*MSLFlight +coeffs(2);
figure(3)
clf('reset')
plot( TimeShiftGPS, AltGPS, '.', AltTimeFlight, altMSLCorr, TimeOR, AltOR+mean(MSLPre))
title('Plot Altitude vs Time for GPS, Corrected Altimeter, and Open Rocket')
xlabel('time (s)')
ylabel('Altitude (m)')
legend('GPS','Corr Altimeter', 'OR Alt')

%% Get Magnetometer Initial Orientation
% Get the magnetometer pre-launch data and calculate the baselines.
% We've skipped getting the pre-launch data as separate variables.
magX0 = mean(MagxPre);
magY0 = mean(MagyPre);
magZ0 = mean(MagzPre);
mag0 = norm([magX0 magY0 magZ0]);
magX0 = magX0/mag0;
magY0 = magY0/mag0;
magZ0 = magZ0/mag0;
magVec0 = [magX0; magY0; magZ0];

% Include the earth magnetic vector from https://www.ngdc.noaa.gov/geomag-web/#igrfwmm
% Use the pre-launch GPS Baselines from above to look up the vectors.
% N 34.4962, W 116.9593, MSL 861.9322 m, 9 July 2017
magN = 23491.6;
magE = 4926.6;
magD = 40795.3;
magUp = - magD;
magEarthVec = [magE; magN; magUp];
magEarthNorm = norm(magEarthVec);
magEarthVecNorm = magEarthVec/magEarthNorm;

%% Calculate roll from magnetometer
rollMag = atan2(MagxBoost,MagzBoost);
fullRollMag = unwrap(rollMag);
% Calculate the variance in roll angle measurements
rollVar = var(atan2(MagxPre,MagzPre));

figure(4)
clf('reset')
plot(MagTimeBoost,180/pi*rollMag)
title('Wrapped Roll Angle from Magnetometer')
xlabel('time (s)')
ylabel('Angle (°)')


figure(5)
clf('reset')
plot(MagTimeBoost,180/pi*fullRollMag)
title('Unwrapped Roll Angle from Magnetometer')
xlabel('time (s)')
ylabel('Angle (°)')

%% Calculate roll from Gyro
fullRollGyro = cumtrapz(GyroTimeBoost,GyroyBoost);

figure(4)
clf('reset')
plot(GyroTimeBoost, 180/pi*fullRollGyro, MagTimeBoost,180/pi*fullRollMag)
title('Roll Angle Comparison from Magnetometer and Rate Gyro')
xlabel('time (s)')
ylabel('Angle (°)')
legend('Gyro', 'Mag')

figure(5)
clf('reset')
plot(GyroTimeBoost,180/pi*(fullRollMag-fullRollGyro))
title('Difference in Roll Angles')
xlabel('time (s)')
ylabel('Angle (°)')

% Next step is to implement Kalman Filter or Smoothing Filter for sensor
% fusion.

%% 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)
CDstr = string(CD);
txt = strcat('C_D = ', CDstr);
xpos = max(vsq)/4;
ypos = max(yCD)/2;
figure(5)
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')
text(xpos,ypos,txt)

%% Calculate CD vs v
% Calculate the CD versus velocity from the data.
CDarr = yCD./vsq;
% Compare CD vs velocity for actual data with Open Rocket.
figure(6)
clf('reset')
plot(velCD,CDarr,'.',VelOR,CDOR)
axis([ 0 inf 0 2])
title('Plot C_D vs v')
xlabel('v (m/s)')
ylabel('C_D')
legend('Flight Data', 'Open Rocket')

%% 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(7)
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)')

%% Load Thrust Curve from outside
% Below are several thrust curves downloaded from Thrustcurve.org and
% massaged into a suitabl form.
% Uncomment the one you want to use, or add your own.
% ThrTime = [0 0.01 0.02 0.06 0.1 0.2 0.4 0.6 0.8 1 1.32 1.55 1.7 1.87 2 2.1]; % J530 data 
% Thr = [0 30 700 790 800 760 725 695 680 658 600 400 180 175 75 0]; % J530 Data
% ThrTime = [0 0.032 0.11 0.161 0.267 0.351 0.512 0.682 1.318 1.461 1.623 1.934 2.039 2.177 2.317 2.5]; % J510 Data
% Thr = [0 1055.904 865.54 829.933 794.325 780.63 705.306 654.633 624.504 475.225 345.121 153.387 115.04 41.086 5.478 0]; % J530 Data
ThrTime = [0	0.006	0.014	0.015	0.018	0.029	0.039	0.047	0.056	0.08	0.104 0.134	0.17	0.202	0.303	0.401	0.497	0.603	0.702	0.8	0.906	0.933	0.952	0.98 1.005	1.01	1.019	1.034	1.055	1.07	1.093	1.115]; % J825R data
Thr = [0	444.822	889.644	1060.299	1202.978	1040.716	1110.656	1071.489 1091.073	1077.085	1068.692	1063.097	1063.097	1068.692	1054.704	1021.132	995.954	948.394	909.227	842.084	772.144	749.763	671.429	579.108	470.001	405.655	405.655	226.607	123.095	50.357	22.381	0];
% ThrTime = [0 0.039 0.119 0.199 0.279 0.36 0.441 0.521 0.602 0.682 0.762 0.844 0.924 1.004 1.084 1.165 1.246 1.326 1.406 1.487 1.568 1.648 1.729 1.809 1.89 1.971 2.052]; %J570 data
% Thr = [0 1149.795 1042.846 960.891 900.02 837.772 792.834 735.51 685.857 649.599 608.757 597.35 568.934 548.552 505.08 484.626 452.328 362.439 297.973 262.381 195.696 156.733 124.649 113.749 69.812 46.023 0];

% Plot the results of the thrust curve and mass loss curve.
ImpStr = string(impulse);
txt = strcat('Impulse = ', ImpStr, ' Ns.');
ypos = max(Thr)/3;
xpos = max(timeTC)/5;
figure(7)
clf('reset')
yyaxis left
plot(timeTC,Tx, ThrTime, Thr)
title('Thrust Curve and Mass Change')
xlabel('time (s)')
ylabel('Thrust (N)')
text(xpos,ypos,txt)
yyaxis right
plot(timeTC,mass)
ylabel('Rocket Mass (kg)')
legend('Measured','Mfg Spec', 'Mass')

%% Sample Temperature at TimeFlight times
T1Resample = interp1(ADCTimeFlight,TADC1Flight,AltTimeFlight,'pchip');
T2Resample = interp1(ADCTimeFlight,TADC2Flight,AltTimeFlight,'pchip');
figure(8)
clf('reset')
plot(altMSLCorr,T1Resample,altMSLCorr,T2Resample)
title('Temperature vs Altitude')
xlabel('Altitude (m MSL)')
ylabel('Temperature ( {\circ}C)')
legend('GPS','Altimeter', 'OR Alt')
% Add code to select the descent data, fit a line to determine the lapse
% rate, and see how close it is to -0.0065 K/m.
% Also add code to plot T vs Velocity during boost phase.
