% Plots and saves a 3d rendering of the aircraft path and attitude
% throughout the maneuver. Pass/fail conditions are displayed in the image.
% This script requires that "SimResults.mat" exists in the workspace. This
% .mat file is automatically generated by RunF16Sim with the variable
% "analysisOn" set to true.
%
% <a href="https://github.com/pheidlauf/AeroBenchVV">AeroBenchVV</a>
% Copyright: GNU General Public License 2017
%
% See also: RUNF16SIM, NEW_OBJECT, FLYPATH

%% Create animation of F16 Maneuver
close all;
% Navigate to flypath package
cd('../flypath3d');
% Run package setup
package_setup;
% Return to Sim folder
cd('../F16_Sim');
load('SimResults');

% Flypath3D Trajectory Visualization
data = output.states.x_f16_hist([11 10 12 5 6 4],:);
tvec = output.states.time;
t_maneuver = output.t_maneuver;
time = output.states.time;
x_f16_hist = output.states.x_f16_hist;

% Flypath3D Trajectory Visualization

% create an object
new_object('aircraft.mat',data,...
    'model','f-16.mat',...
    'edge',[0 0 0],'face',[0 0 0],'alpha',1,...
    'path','on','pathcolor',[.89 .0 .27],'pathwidth',1,...
    'scale', 25);
% aircraft trajectory visualization
flypath('aircraft.mat',...
    'animate','off','step', 150,...
    'axis','on','axiscolor',[0 0 0],'color',[1 1 1],...
    'font','Georgia','fontsize',6,...
    'view', [-37.5, 30],...
    'window',[1000 1000]);
hold on;

% Get indices of maneuver times in result time steps
for (i=1:length(t_maneuver))
    if(t_maneuver(i) >= 0)
        time_maneuver(i) = find(time<=t_maneuver(i),1,'last');
    else
        time_maneuver(i) = 1;
        warning('Maneuver Not Completed');
    end
end

scatter3(x_f16_hist(11,time_maneuver),...
    x_f16_hist(10,time_maneuver),...
    x_f16_hist(12,time_maneuver),...
    50,[0 1 0],...
    'filled');


% Draw altitude floor
ground = output.flightLimits.altitudeMin;
xl = round(xlim);
yl = round(ylim);
[x_mesh,y_mesh] = meshgrid(xl(1):100:xl(2),yl(1):100:yl(2));
z_mesh = ones(size(x_mesh))*ground;
h_surf = surf(x_mesh,y_mesh,z_mesh,...
    'FaceColor',[.8 .8 .8],...
    'EdgeColor',[.7 .7 .7],...
    'FaceAlpha',1,...                                 
    'EdgeAlpha',1);
xlim(xl);
ylim(yl);
        
% Add title with info
str1 = sprintf('Simulation Time: (%2.0f:%2.0f) sec',...
    output.states.time(1),output.states.time(end));
str2 = sprintf('V(0) = %5.2f ft/s   h(0) = %7.2f ft',...
    output.states.VT(1), output.states.h(1));
str3 = sprintf('[\\phi \\theta \\psi](0) = [ %5.1f, %5.1f, %5.1f ] deg ',...
    rad2deg(output.states.phi(1)),...
    rad2deg(output.states.theta(1)),...
    rad2deg(output.states.psi(1)));

% Build pass/fail string
if(output.passFail.stable)
    str4a = sprintf('{\\color{green}Stability} ');
else
    str4a = sprintf('{\\color{red}Stability} ');
end

if(output.passFail.altitude)
    str4b = sprintf('{\\color{green}Altitude} ');
else
    str4b = sprintf('{\\color{red}Altitude} ');
end

if(output.passFail.maneuverTime)
    str4c = sprintf('{\\color{green}Time} ');
else
    str4c = sprintf('{\\color{red}Time} ');
end

str4 = sprintf('Pass/Fail: %s %s %s',str4a, str4b, str4c);            
titlestr = {str1, str2,str3,str4};

title(titlestr, 'FontSize', 16, 'FontName','FixedWidth');

% Save Figure
print('output_picture','-dpng')
close all;