%% Parse File
[fname, pname] = uigetfile('*.TXT','Select file containing Teensy IMU Data strings');
fileID = fopen([pname,fname],'r');
i=1; j=1; k=1; l=1; m=1; n=1; p=1;
while ~feof(fileID)
    line = fgetl(fileID);
    switch line(1:2)
        case 'A	'
            Accel{i} = textscan(line, '%s %f %f %f %f');
            i = i + 1;
        case 'G	'
            GyroRate{j}=textscan(line, '%s %f %f %f %f');
            j = j + 1;
        case 'M	'
            MagVec{k} = textscan(line, '%s %f %f %f %f');
            k = k + 1;
        case 'Al'
            Altitude{l} = textscan(line, '%s %f %s %f %s %f');
            l = l + 1;
        case 'HG'
            HighAccel{m} = textscan(line, '%s %f %f %f %f');
            m = m + 1;
        case 'AD'
            if line(9:10) == '35'
                Pin35{n} = textscan(line, '%s %s %f %f');
                n = n + 1;
            elseif line(9:10) == '36'
                Pin36{p} = textscan(line, '%s %s %f %f');
                p = p + 1;
            else
                'Bad ADC pin'
            end
        otherwise
            'Bad Line Read'
    end
end
fclose(fileID);

%% Separate Arrays
for index = 1:i-1
    AccelTime(index) = Accel{index}{2};
    Accelx(index) = Accel{index}{3};
    Accely(index) = Accel{index}{4};
    Accelz(index) = Accel{index}{5};
end
AccelTime = AccelTime/1e6;
for index = 1:j-1
    GyroTime(index) = GyroRate{index}{2};
    Gyrox(index) = GyroRate{index}{3};
    Gyroy(index) = GyroRate{index}{4};
    Gyroz(index) = GyroRate{index}{5};
end
GyroTime = GyroTime/1e6;
for index = 1:k-1
    MagTime(index) = MagVec{index}{2};
    Magx(index) = MagVec{index}{3};
    Magy(index) = MagVec{index}{4};
    Magz(index) = MagVec{index}{5};
end
MagTime = MagTime/1e6;
for index = 1:l-1
    AltTime(index) = Altitude{index}{2};
    Press(index) = Altitude{index}{4};
    MSL(index) = Altitude{index}{6};
end
AltTime = AltTime/1e6;
for index = 1:m-1
    HGTime(index) = HighAccel{index}{2};
    HGx(index) = HighAccel{index}{3};
    HGy(index) = HighAccel{index}{4};
    HGz(index) = HighAccel{index}{5};
end
HGTime = HGTime/1e6;
for index = 1:n-1
    AD1Time(index) = Pin35{index}{3};
    AD1(index) = Pin35{index}{4};
end
AD1Time = AD1Time/1e6;
for index = 1:p-1
    AD2Time(index) = Pin36{index}{3};
    AD2(index) = Pin36{index}{4};
end
AD2Time = AD2Time/1e6;

%% Plot Raw Data
figure(1)
plot(AccelTime,Accelx,AccelTime,Accely,AccelTime,Accelz)
title('Accelerometer Data')
xlabel('time (s)')
ylabel('Acceleration (m/s^2)')
legend('a_z', 'a_x', 'a_y')
figure(2)
plot(GyroTime,Gyrox,GyroTime,Gyroy,GyroTime,Gyroz)
title('Rotation Rate Data ')
xlabel('time (s)')
ylabel('Rotation Rate (Degrees/s)')
legend('\omega_z', '\omega_x', '\omega_y')
figure(3)
plot(MagTime,Magx,MagTime,Magy,MagTime,Magz)
title('Raw Magnetometer Data')
xlabel('time (s)')
ylabel('Magnetic Field Strength')
legend('m_x', 'm_y', 'm_z')
figure(4)
plot(HGTime,HGx,HGTime,HGy,HGTime,HGz)
title('High-G Accelerometer Data')
xlabel('time (s)')
ylabel('Acceleration (m/s^2)')
legend('a_z', 'a_x', 'a_y')
figure(5)
yyaxis right
plot(AltTime,Press)
title('Altimeter Data')
xlabel('time (s)')
ylabel('Pressure (Pa)')
yyaxis left
plot(AltTime,MSL)
ylabel('Altitude MSL (m)')
legend('MSL', 'P')
figure(6)
plot(AD1Time,AD1,AD2Time,AD2)
title('AD Data')
xlabel('time (s)')
ylabel('Measured Voltage (V)')
legend('AD1', 'AD2')

%% Set Zero and End
t0 = 362.35; % Set the starting value for the flight by zooming in on the plots
ta = 371.3; % Set the apogee time
tf = 459; % 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; %

%% Get Accelerometer Pre, Flight, and Post 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);

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

AccelTimeFlight = AccelTime(it0:itf);
AccelxFlight = Accelx(it0:itf);
AccelyFlight = Accely(it0:itf);
AccelzFlight = Accelz(it0:itf);

AccelTimeBoost = AccelTime(it0:ita);
AccelxBoost = Accelx(it0:ita);
AccelyBoost = Accely(it0:ita);
AccelzBoost = Accelz(it0:ita);

AccelTimeDesc = AccelTime(ita:itf);
AccelxDesc = Accelx(ita:itf);
AccelyDesc = Accely(ita:itf);
AccelzDesc = Accelz(ita:itf);

AccelTimePost = AccelTime(itf:itpo);
AccelxPost = Accelx(itf:itpo);
AccelyPost = Accely(itf:itpo);
AccelzPost = Accelz(itf:itpo);

% Recenter time arrays to be based around liftoff
AccelTimePre = AccelTimePre - t0;
AccelTimeFlight = AccelTimeFlight - t0;
AccelTimeBoost = AccelTimeBoost - t0;
AccelTimeDesc = AccelTimeDesc - t0;
AccelTimePost = AccelTimePost - t0;

% 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 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
it0 = find(GyroTime >= t0,1);
ita = find(GyroTime >= ta,1);
itf = find(GyroTime >= tf,1);
itpr = find(GyroTime >= tpr,1);
itpo = find(GyroTime >= tpo,1);

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

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

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

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

GyroTimePost = GyroTime(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
it0 = find(MagTime >= t0,1);
ita = find(MagTime >= ta,1);
itf = find(MagTime >= tf,1);
itpr = find(MagTime >= tpr,1);
itpo = find(MagTime >= tpo,1);

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

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

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

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

MagTimePost = MagTime(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
it0 = find(HGTime >= t0,1);
ita = find(HGTime >= ta,1);
itf = find(HGTime >= tf,1);
itpr = find(HGTime >= tpr,1);
itpo = find(HGTime >= tpo,1);

HGTimePre = HGTime(itpr:it0);
HGxPre = HGx(itpr:it0);
HGyPre = HGy(itpr:it0);
HGzPre = HGz(itpr:it0);

HGTimeFlight = HGTime(it0:itf);
HGxFlight = HGx(it0:itf);
HGyFlight = HGy(it0:itf);
HGzFlight = HGz(it0:itf);

HGTimeBoost = HGTime(it0:ita);
HGxBoost = HGx(it0:ita);
HGyBoost = HGy(it0:ita);
HGzBoost = HGz(it0:ita);

HGTimeDesc = HGTime(ita:itf);
HGxDesc = HGx(ita:itf);
HGyDesc = HGy(ita:itf);
HGzDesc = HGz(ita:itf);

HGTimePost = HGTime(itf:itpo);
HGxPost = HGx(itf:itpo);
HGyPost = HGy(itf:itpo);
HGzPost = HGz(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;

% Plot pre data
figure(1)
plot(HGTimePre,HGxPre,HGTimePre,HGyPre,HGTimePre,HGzPre)
title('High-G Accelerometer Data Pre')
xlabel('time (s)')
ylabel('Acceleration (m/s^2)')
legend('a_z', 'a_x', 'a_y')

% Plot flight data
figure(2)
plot(HGTimeFlight,HGxFlight,HGTimeFlight,HGyFlight,HGTimeFlight,HGzFlight)
title('High-G Accelerometer Data Flight')
xlabel('time (s)')
ylabel('Acceleration (m/s^2)')
legend('a_z', 'a_x', 'a_y')

% Plot boost data
figure(3)
plot(HGTimeBoost,HGxBoost,HGTimeBoost,HGyBoost,HGTimeBoost,HGzBoost)
title('High-G Accelerometer Data Boost')
xlabel('time (s)')
ylabel('Acceleration (m/s^2)')
legend('a_z', 'a_x', 'a_y')

% Plot 
figure(4)
plot(HGTimeDesc,HGxDesc,HGTimeDesc,HGyDesc,HGTimeDesc,HGzDesc)
title('High-G Accelerometer Data Descent')
xlabel('time (s)')
ylabel('Acceleration (m/s^2)')
legend('a_z', 'a_x', 'a_y')

figure(5)
clf('reset')
plot(HGTimePost,HGxPost,HGTimePost,HGyPost,HGTimePost,HGzPost)
title('High-G Accelerometer Data Post')
xlabel('time (s)')
ylabel('Acceleration (m/s^2)')
legend('a_z', 'a_x', 'a_y')

%% Get Altimeter Pre, Flight, and Post data
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);
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
it0 = find(AD1Time >= t0,1);
ita = find(AD1Time >= ta,1);
itf = find(AD1Time >= tf,1);
itpr = find(AD1Time >= tpr,1);
itpo = find(AD1Time >= tpo,1);

AD1TimePre = AD1Time(itpr:it0);
AD1Pre = AD1(itpr:it0);

AD1TimeFlight = AD1Time(it0:itf);
AD1Flight = AD1(it0:itf);

AD1TimeBoost = AD1Time(it0:ita);
AD1Boost = AD1(it0:ita);

AD1TimeDesc = AD1Time(ita:itf);
AD1Desc = AD1(ita:itf);

AD1TimePost = AD1Time(itf:itpo);
AD1Post = AD1(itf:itpo);

% Recenter time arrays to be based around liftoff
AD1TimePre = AD1TimePre - t0;
AD1TimeFlight = AD1TimeFlight - t0;
AD1TimeBoost = AD1TimeBoost - t0;
AD1TimeDesc = AD1TimeDesc - t0;
AD1TimePost = AD1TimePost - t0;

% repeat for ADC2
it0 = find(AD2Time >= t0,1);
ita = find(AD2Time >= ta,1);
itf = find(AD2Time >= tf,1);
itpr = find(AD2Time >= tpr,1);
itpo = find(AD2Time >= tpo,1);

AD2TimePre = AD2Time(itpr:it0);
AD2Pre = AD2(itpr:it0);

AD2TimeFlight = AD2Time(it0:itf);
AD2Flight = AD2(it0:itf);

AD2TimeBoost = AD2Time(it0:ita);
AD2Boost = AD2(it0:ita);

AD2TimeDesc = AD2Time(ita:itf);
AD2Desc = AD2(ita:itf);

AD2TimePost = AD2Time(itf:itpo);
AD2Post = AD2(itf:itpo);

% Recenter time arrays to be based around liftoff
AD2TimePre = AD2TimePre - t0;
AD2TimeFlight = AD2TimeFlight - t0;
AD2TimeBoost = AD2TimeBoost - t0;
AD2TimeDesc = AD2TimeDesc - t0;
AD2TimePost = AD2TimePost - t0;

% Plot pre data
figure(1)
clf('reset')
plot(AD1TimePre,AD1Pre,AD2TimePre,AD2Pre)
title('AD Data Prelaunch')
xlabel('time (s)')
ylabel('Measured Voltage (V)')
legend('AD1', 'AD2')

% Plot flight data
figure(2)
clf('reset')
plot(AD1TimeFlight,AD1Flight,AD2TimeFlight,AD2Flight)
title('AD Data Flight')
xlabel('time (s)')
ylabel('Measured Voltage (V)')
legend('AD1', 'AD2')

% Plot boost data
figure(3)
clf('reset')
plot(AD1TimeBoost,AD1Boost,AD2TimeBoost,AD2Boost)
title('AD Data Boost')
xlabel('time (s)')
ylabel('Measured Voltage (V)')
legend('AD1', 'AD2')

% Plot 
figure(4)
clf('reset')
plot(AD1TimeDesc,AD1Desc,AD2TimeDesc,AD2Desc)
title('AD Data Descent')
xlabel('time (s)')
ylabel('Measured Voltage (V)')
legend('AD1', 'AD2')

figure(5)
clf('reset')
plot(AD1TimePost,AD1Post,AD2TimePost,AD2Post)
title('AD Data Post Flight')
xlabel('time (s)')
ylabel('Measured Voltage (V)')
legend('AD1', 'AD2')

%% Get GPS Data
% Parse Featherweight GPS Data
i = 1;
[fname, pname] = uigetfile('*.csv','Select file containing Teensy GPS Data strings');
fileID = fopen([pname,fname],'r');
line = fgetl(fileID);
while ~feof(fileID)
    line = fgetl(fileID);
    GPS{i} = textscan(line, '%s', 'Delimiter',',');
    time_gps(i) = datetime(GPS{i}{1}{1},'InputFormat','MMMM d yyyy HH:mm:ss.SSS XXX','TimeZone','UTC');
    unixtime(i) = str2double(GPS{i}{1}{2});
    alt(i) = str2double(GPS{i}{1}{3});
    lat(i) = str2double(GPS{i}{1}{4});
    lon(i) = str2double(GPS{i}{1}{5});
    nsat(i) = str2double(GPS{i}{1}{6});
    fix(i) = str2double(GPS{i}{1}{7});
    horzv(i) = str2double(GPS{i}{1}{8});
    vertv(i) = str2double(GPS{i}{1}{9});
    head(i) = str2double(GPS{i}{1}{10});
    flags(i) = hex2dec(GPS{i}{1}{11}(3:4));
    gt40(i) = str2double(GPS{i}{1}{12});
    gt32(i) = str2double(GPS{i}{1}{12});
    gt24(i) = str2double(GPS{i}{1}{14});
    rssi(i) = str2double(GPS{i}{1}{15});
    batt(i) = str2double(GPS{i}{1}{16});
    i = i + 1;
end
% Convert to SI
alt = alt*0.3048;
horzv = horzv*0.3048;
vertv = vertv*0.3048;
%% Plot GPS data
% Set local time zone
time_gps.TimeZone = 'local';
% Zoom in on plot to determine the launch start time
figure(1)
clf('reset')
yyaxis left
plot(time_gps,alt)
title('GPS Altitude and Velocity')
xlabel('time')
ylabel('Altitude MSL (m)')
yyaxis right
plot(time_gps,vertv,time_gps,horzv)
ylabel('Velocity (m/s)')
legend('MSL', 'v_{vert}', 'v_{horiz}')
% Zoom in on plot to determine the launch start time

%% Set Start Times
t0 = unixtime(1) + 0.1; % Set the starting value for the flight by zooming in on the plots
ta = unixtime(1) + 18; % Set the apogee time
tf = unixtime(1) + 152; % 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; %

%% Get GPS Pre, Flight, and Post data
it0 = find(unixtime >= t0,1);
ita = find(unixtime >= ta,1);
itf = find(unixtime >= tf,1);
itpr = find(unixtime >= tpr,1);
itpo = find(unixtime >= tpo,1);

time_gpsPre = unixtime(itpr:it0);
altPre = alt(itpr:it0);
latPre = lat(itpr:it0);
lonPre = lon(itpr:it0);

time_gpsFlight = unixtime(it0:itf);
altFlight = alt(it0:itf);
latFlight = lat(it0:itf);
lonFlight = lon(it0:itf);

time_gpsBoost = unixtime(it0:ita);
altBoost = alt(it0:ita);
latBoost = lat(it0:ita);
lonBoost = lon(it0:ita);

time_gpsDesc = unixtime(ita:itf);
altDesc = alt(ita:itf);
latDesc = lat(ita:itf);
lonDesc = lon(ita:itf);

time_gpsPost = unixtime(itf:itpo);
altPost = alt(itf:itpo);
latPost = lat(itf:itpo);
lonPost = lon(itf:itpo);

% Recenter time arrays to be based around liftoff
time_gpsPre = time_gpsPre - t0;
time_gpsFlight = time_gpsFlight - t0;
time_gpsBoost = time_gpsBoost - t0;
time_gpsDesc = time_gpsDesc - t0;
time_gpsPost = time_gpsPost - t0;

% Plot pre data
figure(1)
clf('reset')
plot(time_gpsPre,altPre,time_gpsPre,latPre,time_gpsPre,lonPre)
title('GPS Position Data Pre')
xlabel('time (s)')
ylabel('Altitude MSL (M) or Lat,Lon (°)')
legend('alt', 'lat', 'lon')

% Plot flight data
figure(2)
clf('reset')
plot(time_gpsFlight,altFlight,time_gpsFlight,latFlight,time_gpsFlight,lonFlight)
title('GPS Position Data Flight')
xlabel('time (s)')
ylabel('Altitude MSL (M) or Lat,Lon (°)')
legend('alt', 'lat', 'lon')

% Plot boost data
figure(3)
clf('reset')
plot(time_gpsBoost,altBoost,time_gpsBoost,latBoost,time_gpsBoost,lonBoost)
title('GPS Position Data Boost')
xlabel('time (s)')
ylabel('Altitude MSL (M) or Lat,Lon (°)')
legend('alt', 'lat', 'lon')

% Plot 
figure(4)
clf('reset')
plot(time_gpsDesc,altDesc,time_gpsDesc,latDesc,time_gpsDesc,lonDesc)
title('GPS Position Data Descent')
xlabel('time (s)')
ylabel('Altitude MSL (M) or Lat,Lon (°)')
legend('alt', 'lat', 'lon')

figure(5)
clf('reset')
plot(time_gpsPost,altPost,time_gpsPost,latPost,time_gpsPost,lonPost)
title('GPS Position Data Post')
xlabel('time (s)')
ylabel('Altitude MSL (M) or Lat,Lon (°)')
legend('alt', 'lat', 'lon')

%% Plot GPS and Altitude
figure(6)
clf('reset')
plot(AltTimeFlight,MSLFlight,time_gpsFlight,altFlight)
title('GPS and Altimeter Altitude Comparison')
xlabel('time (s)')
ylabel('Altitude MSL (M)')
legend('GPS', 'alt')

