% This is sample code put together quickly (no guarantees for correctness
% and for sure is not efficient) to give you some ideas for loading and
% processing data from the Vehicle Dynamics data runs.

clear all

PARSING_NEEDED=0;    %  0 if just want to load the last file you already parsed.

% file name
Date = '2_2_2014_____'
Time = '13_45_25';
folderName = sprintf('VehicleDynamics_%s%s',Date,Time)
fileName = sprintf('%s/%s.alog',folderName,folderName)

% output file name
matFile  = 'newMat';

RAD2DEG=180/pi;
HW_2_SW=15.1;   % ratio from hand wheel to steer wheel.
addpath('GPS_Functions/');

if(PARSING_NEEDED)
    read_async_log(fileName, matFile); % parse the file (will be saved to newMat.mat)
    load(matFile);
else
    load(matFile);
end

% Septentrio Position and Velocity Measurements
j=1;
for k=1:length(gSeptentrio.zECEF_X)
    if(~isnan(gSeptentrio.zECEF_X(k)))
         XYZ(j,:)=[gSeptentrio.zECEF_X(k) gSeptentrio.zECEF_Y(k) gSeptentrio.zECEF_Z(k)];
         XYZ_VEL(j,:)=[gSeptentrio.zECEF_VX(k) gSeptentrio.zECEF_VY(k) gSeptentrio.zECEF_VZ(k)];
        [Lat(j),Long(j),Alt(j)]=wgsxyz2lla(XYZ(j,:));
        [ENU(j,:)]=wgsxyz2enu(XYZ(j,:)', Lat(1), Long(1), Alt(1));
        [ENU_VEL(j,:)]=wgsdiffxyz2diffenu(XYZ_VEL(j,:)',Lat(1),Long(1));
        speed_gps(j)=norm(ENU_VEL(j,1:2));
        t_gps(j)=gSeptentrio.time(k);
        j=j+1;
    end
end

gps_course=atan2(ENU_VEL(:,1),ENU_VEL(:,2));

% Septentrio Attitude Measurements
j=1;
for k=1:length(gSeptentrio.zYaw)
    if(~isnan(gSeptentrio.zYaw(k)))
        GPS_ATT(j,:)=[gSeptentrio.zRoll(k) gSeptentrio.zPitch(k) gSeptentrio.zYaw(k)];
        t_att(j)=gSeptentrio.time(k);
        j=j+1;
    end
end

figure(1)
plot(ENU(:,1),ENU(:,2))
xlabel('East (m)');
ylabel('North (m)');

figure(2)
subplot(2,1,1)
plot(t_gps,speed_gps)
ylabel('GPS Speed (m/s)');
subplot(2,1,2)
plot(t_gps,gps_course*RAD2DEG)
hold on
plot(t_gps,GPS_ATT(:,3)*RAD2DEG,'r');
hold off
legend('GPS Course (deg)','GPS Yaw (deg)')
xlabel('Time (s)');
ylabel('Heading');

% CROSSBOW 400 IMU Measurements
j=1;
for k=1:length(gXbow400.time)
    if(~isnan(gXbow400.zGyroZ(k)))
        r_imu(j,:)=[gXbow400.zGyroX(k) gXbow400.zGyroY(k) gXbow400.zGyroZ(k)];
        a_imu(j,:)=[gXbow400.zAccelX(k) gXbow400.zAccelY(k) gXbow400.zAccelZ(k)];
        t_imu(j)=gXbow400.time(k);
        j=j+1;
    end
end

figure(3)
subplot(2,1,2)
plot(t_imu,a_imu);
ylabel('Crossbow 400 Accels (m/s^2)');
subplot(2,1,1)
plot(t_imu,r_imu*RAD2DEG);
xlabel('Time (s)');
ylabel('Crossbow 400 Gyros (deg/s)');


% CAN Steer Angle Measurements (note since CAN messages come in a different
% rates this must be done for each measurement (or you can simply do:
% >> plot(gG35_2_can.time,gG35_2_can.EngineRPM,'*') 
% which will not plot values that are "NaN"

j=1;
for k=1:length(gG35_2_can.SteerAngle)
    if(~isnan(gG35_2_can.SteerAngle(k)))
        delta(j)=gG35_2_can.SteerAngle(k)/HW_2_SW;
        t_delta(j)=gG35_2_can.time(k);
        j=j+1;
    end
end

figure(4)
plot(t_delta,delta)
xlabel('Time (s)');
ylabel('Tire Steer Angle (deg)');

figure(5)
plot(gG35_2_can.time,gG35_2_can.EngineRPM,'*') 
xlabel('Time (s)');
ylabel('Engine RPM');

