%% LoadFile
% The easiest way to read in the XTRA or BASE .csv is to read it in as
% a table.
AIM_Data=readtable('E190_9SEP2017_J825R_1_XTRA.csv');
% You have to actually look at the column headings to make sure
% you have the correct column names.
% Because the AIM XTRA uses different sample rates for the different
% sensors, most columns (except for the z-axis accelerometer and associated
% time) will have NaNs for much of the column. They show up once the actual
% column data cease. THe easiest way to remove them is with a conditional
% using isfinite.

% The data in a column heading may be accessed by appending with a dot, ".",
% and either the column name, e.g., .time, or the column number in
% parentheses, e.g., .(1). The example below shows how to plot two columns
% using either method of addressing.

% %% PlotDotName
% plot(AIM_Data.time,AIM_Data.GyroZ)
% title('Test Plot Table Columns .name')
% xlabel('time (s)')
% ylabel('\omega_z (°/s)')
% %% PlotDotNumber
% plot(AIM_Data.(1),AIM_Data.(4))
% title('Test Plot Table Columns .(#)')
% xlabel('time (s)')
% ylabel('\omega_z (°/s)')

%% SearchApogee
% Find the apogee time
accelTimePost=AIM_Data.time_5(AIM_Data.time_5>=0);
accelZPost=AIM_Data.acceleration(AIM_Data.time_5>=0);
plot(accelTimePost,accelZPost)
title('z-axis acceleration')
xlabel('time (s)')
ylabel('a_z (g)')

%% SetApogee
apogeeTime = 24.6; % Determined by looking at the above plot and zooming in.
% A clever person may be able to automate the selection process.
sp = 0.001; % Set the sample period
timeArr = 0:sp:apogeeTime; % The Z accelerometer has an approximate 
% sample rate of 100 SPS, or a period of 0.01 s. Using a precise time
% array makes a lot of things simpler. We'll resample later using timeArr.

% Table data are column vectors.
% You should transpose most table data before using.

%% Set T0 and Hr
T0F = 78; % From Flight Data webpage. Temperature in °F at launch site
T0 = 5/9*(T0F-32) + 273.15; %Convert T0 to Kelvin
Hr = 0.54; % From Flight Data webpage. Relative humidity.

%% DisplayTemperatures
% Below is the procedure for calculating the temperatures of the
% thermistors from the voltages in Channels C & D. Note the transposition
% and the isfinite to remove NaNs.
VoltC=transpose(AIM_Data.CVoltage(isfinite(AIM_Data.CVoltage)));
[TCC,TCK]=TfromV(VoltC);
TtimeC=transpose(AIM_Data.time_3(isfinite(AIM_Data.time_3)));
VoltD=transpose(AIM_Data.DVoltage(isfinite(AIM_Data.DVoltage)));
[TDC,TDK]=TfromV(VoltD);
TtimeD=transpose(AIM_Data.time_4(isfinite(AIM_Data.time_4)));
plot(TtimeC,TCC,TtimeD,TDC)
title('Temperature vs. Time Profile')
xlabel('time (s)')
ylabel('Temperature (°C)')
legend('Channel C','Channel D')

%% GetGPSdata
timeGPS=transpose(AIM_Data.time_8(isfinite(AIM_Data.time_8)));
timeGPSDesc = timeGPS(timeGPS>apogeeTime);
lat=transpose(AIM_Data.lat(isfinite(AIM_Data.lat)));
long=transpose(AIM_Data.long(isfinite(AIM_Data.long)));
GPSMSL=transpose(AIM_Data.GPSMSL(isfinite(AIM_Data.GPSMSL)));
GPSMSLDesc = GPSMSL(timeGPS>apogeeTime);
plot(timeGPS,lat, 'g*', timeGPS,long, 'b*', timeGPS,GPSMSL, 'r*')
title('GPS Data')
xlabel('time (s)')
ylabel('Latitude (°), Longitude (°), Altitude (m MSL)')
legend('lat', 'long', 'alt')

%% CalcGPSBaseline
latBase = mean(lat(timeGPS<0)); % Pre-launch lat, for mag vec & gravity
longBase = mean(long(timeGPS<0)); %Pre-launch long, for mag vec
GPSMSLBase = mean(GPSMSL(timeGPS<0)); % Pre-launch altitude for mag vec, gravity, and AGL
h0 = GPSMSLBase; % Correction for barometric altitude

%% CalcGPSPositionChanges
latDel = 110574*(lat-latBase); % Distance in m
longDel = 111320*(long-longBase).*cos(pi/180*(lat+latBase)/2); % Distance in m
GPSAGL = GPSMSL - GPSMSLBase;
plot(timeGPS,longDel, 'g*', timeGPS,latDel, 'b*', timeGPS,GPSAGL, 'r*') % 
title('GPS Data converted to Position Change')
xlabel('time (s)')
ylabel('Distance (m)')
legend('x', 'y', 'AGL') % 

%% GetAccelData
timeAccel=transpose(AIM_Data.time_5(isfinite(AIM_Data.time_5)));
accelZ=transpose(AIM_Data.acceleration(isfinite(AIM_Data.acceleration)));
timeAccelLat=transpose(AIM_Data.time_6(isfinite(AIM_Data.time_6)));
accelX=transpose(AIM_Data.lat_XAccel_(isfinite(AIM_Data.lat_XAccel_)));
accelY=transpose(AIM_Data.lat_YAccel_(isfinite(AIM_Data.lat_YAccel_)));
plot(timeAccel,accelZ, timeAccelLat,accelX, timeAccelLat,accelY)
title('Accelerometer Data')
xlabel('time (s)')
ylabel('Acceleration (g''s)')
legend('a_z', 'a_x', 'a_y')

% Get local gravitational field from https://www.sensorsone.com/local-gravity-calculator/
%  N 34.4962, GPS alt 861.9322 m
localg = 9.79425;

%% ConvertUnits
% Convert accel data to meters per second.
accelX=accelX*9.80665;
accelY=accelY*9.80665;
accelZ=accelZ*9.80665;

%% AccelPreLaunch
% Get the accel pre-launch data. Unlike the gyro data, it's not easy to use
% these data to calculate baselines or offsets unless you're certain that
% the rocket is perfectly vertical on the pad. However, if you've
% determined the baselines separately, these are really useful for the
% TRIAD method gravity vector.
accelZTimePre=timeAccel(timeAccel<0);
accelZPre=accelZ(timeAccel<0);
accelXYTimePre=timeAccelLat(timeAccelLat<0);
accelXPre=accelX(timeAccelLat<0);
accelYPre=accelY(timeAccelLat<0);
plot(accelZTimePre,accelZPre,accelXYTimePre,accelXPre,accelXYTimePre,accelYPre)
title('Pre-Launch Accelerometer Data')
xlabel('time (s)')
ylabel('Acceleration (m/s)')
legend('a_z', 'a_x', 'a_y')

% AccelPadVector
accelPreVec = [mean(accelXPre); mean(accelXPre); mean(accelZPre)];

%% Resample
% Get the accel data between launch and apogee by resampling at the timeArr
% values.
accelZrs = interp1(timeAccel,accelZ,timeArr,'pchip');
accelXrs = interp1(timeAccelLat,accelX,timeArr,'pchip');
accelYrs = interp1(timeAccelLat,accelY,timeArr,'pchip');

% Create the set of local measured acceleration vectors. They are column
% vectors of the form [ax; ay; az] and the column index corresponds to the
% index of a given timeArr value
accelVec = [accelXrs; accelYrs; accelZrs]; % Set of acceleration vectors


%% InitiateLoop

% Initiate the z acceleration and velocity vectors.
vel1DLoc(1) = 0;
vel1DRaw(1) = 0;
z1DRaw(1)=0;

%% RunLoop
% Prep for the for loop.
n = length(timeArr);
for i = 1:n-1
    vel1DRaw(i+1) = vel1DRaw(i) +  sp*(accelZrs(i+1)-localg);
    z1DRaw(i+1) = z1DRaw(i) + sp*vel1DRaw(i+1);
end

%% PlotZAcceleration
% Compare raw accelerometer data, raw with g subtracted
plot(timeArr,accelZrs,timeArr,accelZrs-localg)
title('z Acceleration Data')
xlabel('time (s)')
ylabel('Acceleration (m/s^2)')
legend('raw', 'raw-g')


%% PlotZVelocity
% Compare Integral of
% Local z acceleration with smoothing, Locak z acceleration with Kalman
plot(timeArr,vel1DRaw)
title('z Velocity Data')
xlabel('time (s)')
ylabel('Acceleration (m/s^2)')
legend('Raw z')

%% PlotZPosition
plot(timeArr,z1DRaw)
title('Accel Altitude Data')
xlabel('time (s)')
ylabel('Position (m)')
legend('z')

%% GetAltimeterdata
timeAlt=transpose(AIM_Data.time_7(isfinite(AIM_Data.time_7)));
Pdata=transpose(AIM_Data.pressure(isfinite(AIM_Data.pressure)));
altMSL=transpose(AIM_Data.PressureMSL(isfinite(AIM_Data.PressureMSL)));
altAGL=transpose(AIM_Data.PressureAGL(isfinite(AIM_Data.PressureAGL)));
plot(timeAlt,altMSL, timeAlt,altAGL)
title('Pressure Altimeter Data')
xlabel('time (s)')
ylabel('Altitude MSL (m), Altitude AGL (m)')
legend('MSL', 'AGL')

%% Get Altimeter Parts & Sync w GPS
PBase = Pdata(timeAlt<0);
P0 = mean(PBase); % Correction for barometric altitude
altMSLBase = altMSL(timeAlt<0);
altAGLBase = altAGL(timeAlt<0);
timeAltBoost = timeAlt(timeAlt>=0 & timeAlt<=apogeeTime);
timeAltDesc = timeAlt(timeAlt>apogeeTime);
altMSLSyncGPS = interp1(timeAlt,altMSL,timeGPSDesc,'pchip');
plot(timeGPSDesc,GPSMSLDesc, timeGPSDesc,altMSLSyncGPS)
title('GPS and Altimeter Data')
xlabel('time (s)')
ylabel('GPS Altitude MSL (m), Altimeter Altitude MSL (m)')
legend('GPS', 'ALT')

%% Calculate Molecular Weight
PH2O = 1000*Hr*exp(16.3872-3885.7/(T0-42.98)) % Partial pressure of water vapor
Mavg = (0.0289644*(P0-PH2O)+0.01801528*PH2O)/P0 % Average Molecular Weight

%% Calculate barometric altitude corrected for P0, T0, h0, Hr
Lapse = -6.5/1000; %Lapse rate in K/m
R = 8.314462618; % Gas constant in J/mol K
g0 = 9.80665; % gravitational acceleration. Could use local.
hMSL = h0+T0/Lapse*((Pdata/P0).^-(Lapse*R/Mavg/g0)-1);
hAGL = hMSL - h0;
plot(timeAlt,hMSL, timeAlt,hAGL)
title('Corrected Barometric Altimeter Data')
xlabel('time (s)')
ylabel('Barometric Altitude (m)')
legend('hMSL', 'hAGL')

%% Get GPS vs Alt Slope Intercept
coeffs = polyfit(altMSLSyncGPS, GPSMSLDesc, 1)
altMSLCorr = coeffs(1)*altMSL +coeffs(2);
altAGLCorr = altMSLCorr - mean (altMSLCorr(timeAlt<0));
plot(timeAlt,altMSLCorr,'.',timeAlt,altMSL,'.',timeGPS,GPSMSL,'+')
title('GPS and Corrected Altimeter Data')
xlabel('time (s)')
ylabel('Altitude MSL (m)')
legend('corr MSL','raw MSL', 'GPS')

%% Resample Corrected Altitude MSL
altAGLCorrrs = interp1(timeAlt,altAGLCorr,timeArr,'pchip');
hAGLrs = interp1(timeAlt,hAGL,timeArr,'pchip');
altAGLrs = interp1(timeAlt,altAGL,timeArr,'pchip');
plot(timeArr,altAGLCorrrs,timeArr,z1DRaw)
title('Corrected Altimeter Data and Accelerometer Altitude')
xlabel('time (s)')
ylabel('Altitude AGL (m)')
legend('Alt AGL','Accel Alt AGL')

%% Get Temperature vs Altitude
TCCrs = interp1(TtimeC,TCC,timeAltDesc,'pchip');
TDCrs = interp1(TtimeD,TDC,timeAltDesc,'pchip');
altAGLDesc = altAGLCorr(timeAlt>apogeeTime);
plot(altAGLDesc,TCCrs,'.',altAGLDesc,TDCrs,'.')
title('Temperature vs. Altitude')
xlabel('Altitude AGL (m)')
ylabel('Temperature (°C)')
legend('Channel C','Channel D')

%% Plot all of the stuff
plot(timeAlt,altAGL,timeAlt,hAGL,timeGPS,GPSAGL,timeArr,altAGLCorrrs, timeArr, z1DRaw)
title('Corrected Altimeter Data Plots')
xlabel('time (s)')
ylabel('Altitude AGL (m)')
legend('Alt AGL','Alt Cor AGL','GPS AGL','Alt ctGPS AGL','Accel Alt AGL')

%% Plot all of the differences
plot(altAGLCorrrs,altAGLrs-altAGLCorrrs,altAGLCorrrs,hAGLrs-altAGLCorrrs,altAGLCorrrs, z1DRaw-altAGLCorrrs)
title('Altimeter Differences')
xlabel('GPS Corr Alt (m)')
ylabel('Difference in Altitude (m)')
legend('Alt AGL - GPS','Alt Cor AGL - GPS','Accel Alt AGL - GPS')

%% OpenAndFixFile
% Open and read the csv file. The Raven software will let you export the
% Raven data as a csv file. It's formatted incorrectly, but you can read
% it. Change the file below to your actual Raven .csv file. Not every
% single column is plotted, but some representative data are. You can plot
% additional data by looking in the FIP_Data table at the headings and
% making the substitutions in the relevant locations.
% Note that bILBA in a column label indicated Ignition, Lift-off, Burnout,
% Apogee in a binary format (one-hot binary encoding).
fid  = fopen('E190_9SEP2017_J825R_1_Raven.csv','r');
f=fread(fid,'*char')';
fclose(fid);

% Fix the ,,-to-,,, problem and write temporary file
goodFile = replace(f,",,",",,,");
fid  = fopen('temp.csv','w');
fprintf(fid,'%s',goodFile);
fclose(fid);

% Read the temporary file into a table
FIP_Data=readtable('temp.csv');
clearvars f goodFile fid

%% GetPlotAccelData
timeAccel=transpose(FIP_Data.Time_AxialAccel_Gs_(isfinite(FIP_Data.Time_AxialAccel_Gs_)));
dtA = mean(diff(timeAccel))
accelZ=transpose(FIP_Data.AxialAccel_Gs_(isfinite(FIP_Data.AxialAccel_Gs_)));
timeAccelLat=transpose(FIP_Data.Time_LateralAccel_Gs_(isfinite(FIP_Data.Time_LateralAccel_Gs_)));
dtAL = mean(diff(timeAccelLat))
accelX=transpose(FIP_Data.LateralAccel_Gs_(isfinite(FIP_Data.LateralAccel_Gs_)));
plot(timeAccel,accelZ,timeAccelLat,accelX)
title('Accelerometer Data')
xlabel('Time (s)')
ylabel('Acceleration (g)')
legend('a_x', 'a_y')

%% Correct and Integrate Accel Data
accelZtrue = g0*accelZ - localg;
velR = cumtrapz(timeAccel,accelZtrue);
altAccelR = cumtrapz(timeAccel,velR);
plot(timeAccel,altAccelR)
title('Altitude Data from Accelerometer')
xlabel('Time (s)')
ylabel('Altitude AGL (m)')
legend('Alt Accel AGL')

%% GetPlotBaroAtmData
timeBaro=transpose(FIP_Data.Time_Baro_Atm_(isfinite(FIP_Data.Time_Baro_Atm_)));
dtB = mean(diff(timeBaro))
Baro=transpose(FIP_Data.Baro_Atm_(isfinite(FIP_Data.Baro_Atm_)));
BaroPa = 101325*Baro; % Convert to Pa
plot(timeBaro,BaroPa)
title('Barometric Pressure Data')
xlabel('Time (s)')
ylabel('Barometric Pressure (Pa)')
legend('Pressure')

%% GetPlotAltitudeData
timeAltB=transpose(FIP_Data.Time__Altitude_Baro_Ft_AGL__(isfinite(FIP_Data.Time__Altitude_Baro_Ft_AGL__)));
dtAB = mean(diff(timeAltB)) % The altitude calculated from barometer
altB=transpose(FIP_Data.x_Altitude_Baro_Ft_AGL__(isfinite(FIP_Data.x_Altitude_Baro_Ft_AGL__)));
altBM = 0.3048*altB;
plot(timeAltB,altBM)
title('Altitude Data')
xlabel('Time (s)')
ylabel('Altitude AGL (m)')
legend('Alt Baro AGL')

%% Fix Baro Data for P0, T0, h0, Hr
hMSLR = h0+T0/Lapse*((BaroPa/P0).^-(Lapse*R/Mavg/g0)-1);
hAGLR = hMSLR - h0;
plot(timeAltB,hAGLR)
title('Raven Altitude Data Corrected for P0, T0, h0, Hr')
xlabel('Time (s)')
ylabel('Altitude AGL (m)')
legend('Alt Baro Corr AGL')
