%% 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 plot and zooming in.
% A clever person may be able to automate the selection process.
timeArr = 0:0.01: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.

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

%% GetGyroData
% Getting IMU data. Note the transposition and NaN removal.
timeGyro=transpose(AIM_Data.time(isfinite(AIM_Data.time)));
gyroX=transpose(AIM_Data.GyroX(isfinite(AIM_Data.GyroX)));
gyroY=transpose(AIM_Data.GyroY(isfinite(AIM_Data.GyroY)));
gyroZ=transpose(AIM_Data.GyroZ(isfinite(AIM_Data.GyroZ)));
plot(timeGyro,gyroZ, timeGyro,gyroX, timeGyro,gyroY)
title('Gyro Data')
xlabel('time (s)')
ylabel('Rotation Rate (°/s)')
legend('\omega_z', '\omega_x', '\omega_y')

%% 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')
legend('a_z', 'a_x', 'a_y')

%% ConvertUnits
% Convert IMU data to radians per second and meters per second.
gyroX=gyroX*pi/180;
gyroY=gyroY*pi/180;
gyroZ=gyroZ*pi/180;
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')
legend('a_z', 'a_x', 'a_y')

%% Resample
% Get the accel data between launch and apogee by resamling 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

% Get the gyro pre-launch data and calculate the baselines or offsets.
% We've skipped getting the pre-launch data as separate variables.
gyroXbase = mean(gyroX(timeGyro<0));
gyroYbase = mean(gyroY(timeGyro<0));
gyroZbase = mean(gyroZ(timeGyro<0));

% Get the gyro data between launch and apogee by resamling at the timeArr
% values.
gyroXrs = interp1(timeGyro,gyroX,timeArr,'pchip');
gyroYrs = interp1(timeGyro,gyroY,timeArr,'pchip');
gyroZrs = interp1(timeGyro,gyroZ,timeArr,'pchip');

% Create the set of local measured rotation-rate vectors. They are column
% vectors of the form [wx; wy; wz] and the column index corresponds to the
% index of a given timeArr value
rotRateVec = [gyroXrs-gyroXbase; gyroYrs-gyroYbase; gyroZrs-gyroZbase];

%% InitiateLoop
% Initiate quaternion. This is the point to apply the TRIAD method to
% determine the initial alignment of the rocket on the pad with ENU.
qr(1:4,1)=[1; 0; 0; 0]; % Initialize the rotation quaternion array

% Initiate the global acceleration, velocity and position vectors.
accelGlobVec(1:3,1) = [0;0;0];
velGlobVec(1:3,1) = [0;0;0];
posGlobVec(1:3,1) = [0;0;0];

% Initiate the local corrected acceleration and velocity vectors.
accelLocVec(1:3,1) = [0;0;0]; % Used for thrust calculations.
velLocVec(1:3,1) = [0;0;0]; % Used for drag calculations.
velLoc(1) = 0;

%% RunLoop
% Prep for the for loop.
n = length(timeArr);
for i =1:n-1
    % Calculate the qdot quaternion
    qdot = qdotCalc(qr(1:4,i),rotRateVec(1:3,i));
    % Take one explicit Euler step forward with rotation quaternion
    qr(1:4,i+1)=qr(1:4,i)+(timeArr(i+1)-timeArr(i))*qdot;
    % Renormalize quaternion
    qr(1:4,i+1) = qr(1:4,i+1)/norm(qr(1:4,i+1));
    % The slightly faster but less accurate method of normalization is
    % commented out below.
    %qr(1:4,i+1)=qr(1:4,i+1)*(1.5-0.5*(qr(1,i+1)^2+qr(2,i+1)^2+qr(3,i+1)^2+...
    %    qr(4,i+1)^2));
    accelGlobVec(1:3,i+1)=rotVecToGlob(accelVec(1:3,i+1),qr(1:4,i+1)) ...
        - [0;0;9.80665]; %You should actually use the local value of g, but
        % since g varies with altitude, it would be hard to do.
    accelLocVec(1:3,i+1) = rotVecToLoc(accelGlobVec(1:3,i+1),qr(1:4,i+1));
    velGlobVec(1:3,i+1) = velGlobVec(1:3,i) + (timeArr(i+1)-timeArr(i)) ...
        *accelGlobVec(1:3,i+1);
    velLocVec(1:3,i+1) = rotVecToLoc(velGlobVec(1:3,i+1),qr(1:4,i+1));
    velLoc(i+1)= norm(velLocVec(1:3,i+1));
    posGlobVec(1:3,i+1) = posGlobVec(1:3,i)+ (timeArr(i+1)-timeArr(i)) ...
        *velGlobVec(1:3,i+1);
end

%% PlotGlobalAcceleration
plot(timeArr,accelGlobVec(1,1:n),timeArr,accelGlobVec(2,1:n), ...
    timeArr,accelGlobVec(3,1:n))
title('Global Acceleration Data')
xlabel('time (s)')
ylabel('Acceleration (m/s^2)')
legend('a_x', 'a_y', 'a_z')

%% PlotGlobalVelocity
plot(timeArr,velGlobVec(1,1:n),timeArr,velGlobVec(2,1:n), ...
    timeArr,velGlobVec(3,1:n))
title('Global Velocity Data')
xlabel('time (s)')
ylabel('Velocity (m/s)')
legend('v_x', 'v_y', 'v_z')

%% PlotGlobalPosition
plot(timeArr,posGlobVec(1,1:n),timeArr,posGlobVec(2,1:n), ...
    timeArr,posGlobVec(3,1:n))
title('Global Position Data')
xlabel('time (s)')
ylabel('Position (m)')
legend('x', 'y', 'z')

%% PlotLocalAcceleration
plot(timeArr,accelLocVec(1,1:n),timeArr,accelLocVec(2,1:n), ...
    timeArr,accelLocVec(3,1:n))
title('Local Acceleration Data')
xlabel('time (s)')
ylabel('Acceleration (m/s^2)')
legend('a_x', 'a_y', 'a_z')

%% PlotLocalVelocity
plot(timeArr,velLocVec(1,1:n),timeArr,velLocVec(2,1:n), ...
    timeArr,velLocVec(3,1:n),timeArr,velLoc)
title('Local Velocity Data')
xlabel('time (s)')
ylabel('Velocity (m/s)')
legend('v_x', 'v_y', 'v_z', 'v_{tot}')

%% GetMagnetometerData
timeMag=transpose(AIM_Data.time_9(isfinite(AIM_Data.time_9)));
magX=transpose(AIM_Data.MagX(isfinite(AIM_Data.MagX)));
magY=transpose(AIM_Data.MagY(isfinite(AIM_Data.MagY)));
magZ=transpose(AIM_Data.MagZ(isfinite(AIM_Data.MagZ)));
plot(timeMag,magX, timeMag,magY, timeMag,magZ)
title('Raw Magnetometer Data')
xlabel('time (s)')
ylabel('Magnetic Field Strength')
legend('m_x', 'm_y', 'm_z')

%% ScaleMAgData
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);
plot(timeMag,magXsc, timeMag,magYsc, timeMag,magZsc)
title('Scaled Magnetometer Data')
xlabel('time (s)')
ylabel('Magnetic Field Strength')
legend('m_x', 'm_y', 'm_z')

%% 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(magXsc(timeMag<0));
magY0 = mean(magYsc(timeMag<0));
magZ0 = mean(magZsc(timeMag<0));
mag0 = norm([magX0 magY0 magZ0]);
magX0 = magX0/mag0;
magY0 = magY0/mag0;
magZ0 = magZ0/mag0;

%% ResampleMag
% Get the magnetometer data between launch and apogee by resamling at the timeArr
% values.
magXrs = interp1(timeMag,magXsc,timeArr,'pchip');
magYrs = interp1(timeMag,magYsc,timeArr,'pchip');
magZrs = interp1(timeMag,magZsc,timeArr,'pchip');

% Create the set of local measured magnetometer vectors. They are column
% vectors of the form [mx; my; mz] and the column index corresponds to the
% index of a given timeArr value
magVec = [magXrs; magYrs; magZrs]; % Set of magnetometer vectors
%renormalize again
for i = 1:n
    magVec(1:3,i) = magVec(1:3,i)/norm(magVec(1:3,i));
    roll(i) = atan2(magVec(2,i),magVec(1,i));
    %tilt(i) = atan2(magVec(3,i),norm([magVec(2,i) magVec(1,i)]));
end
plot(timeArr,magVec(1,1:n),timeArr,magVec(2,1:n),timeArr,magVec(3,1:n))
title('Scaled and Normalized Magnetometer Data')
xlabel('time (s)')
ylabel('Magnetic Field Strength')
legend('m_x', 'm_y', 'm_z')

%% RollPitchYawAngle
rGyro(1) = roll(1);
pGyro(1) = 0; % tilt(1); % Fix starting angle
yGyro(1) = 0; % tilt(1); % Fix starting angle
for i = 1:n-1
    rGyro(i+1) = rGyro(i)+ (timeArr(i+1)-timeArr(i))*(gyroZrs(i)-gyroZbase);
    pGyro(i+1) = pGyro(i)+ (timeArr(i+1)-timeArr(i))*(gyroXrs(i)-gyroXbase);
    yGyro(i+1) = yGyro(i)+ (timeArr(i+1)-timeArr(i))*(gyroYrs(i)-gyroYbase);
end

%% RollPlot
plot(timeArr, 180/pi*unwrap(roll), timeArr, 180/pi*unwrap(rGyro))
title('Unwrapped Roll Angle in Degrees')
xlabel('time (s)')
ylabel('Roll Angle (°)')
legend('magnetometer', 'rate gyro')

%% RollDifference
rollDif = unwrap(roll)-unwrap(rGyro);
plot(timeArr, 180/pi*rollDif)
title('Difference between Magnetometer and Gyro in Degrees')
xlabel('time (s)')
ylabel('Difference in Roll Angle (°)')

%% 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));
longBase = mean(long(timeGPS<0));
GPSMSLBase = mean(GPSMSL(timeGPS<0));

%% 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 ?')
xlabel('time (s)')
ylabel('Distance (m)')
legend('x', 'y', 'AGL') % 

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

%% 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');
plot(timeArr,posGlobVec(3,1:n),timeArr,altAGLCorrrs)
title('Corrected Altimeter Data and IMU Altitude')
xlabel('time (s)')
ylabel('Altitude AGL (m)')
legend('IMU','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')
