function kaiserdiagram(aircraft)
% KAISERDIAGRAM
%
% Plot Kaiser diagram - minimum time-to-climb climb schedule using chebfuns
%
% kaiserdiagram('execujet') -  McClamroch's Executive Jet
% kaiserdiagram('aircraft1') -  Bryson, Desai & Hoffman's Aircraft 1
%
% References
% [1] J.F. Whidborne, ``Solving optimal control problems using the Chebfun
%    system'', UKACC Control 2016, Belfast, Septemeber 2016.
% % [2] F. Kaiser, ``The climb of jet-propelled aircraft, part I. Speed along
%     the path of optimum climb,'' Ministry of Supply (Gt. Brit.), RTP/TIB
%     Translation No. Tech. Rep. GDC/15/14ST, Apr. 1944.
% [3] D.J.N. Limebeer and A.V. Rao, ``Faster, higher, and greener: Vehicular
%     optimal control,'' IEEE Control Systems, vol. 35, no. 2, pp. 36–56, 2015.
%     doi: 10.1109/MCS.2014.2384951
% [4] A.E. Bryson Jr., M.N. Desai, and W.C. Hoffman, ``Energy-state
%     approximation in performance optimization of supersonic aircraft,''
%     Journal of Aircraft, vol. 6, no. 6, 1969.
%     doi: 10.2514/3.44093
% [5] N.H. McClamroch, Steady Aircraft Flight and Performance.
%     Princeton University Press, 2011.
% [6] J.T. Betts, Practical Methods for Optimal Control and Estimation 
%     Using Nonlinear Programming.SIAM, 2001. 
%     doi:  10.1137/1.9780898718577
% [7] A. E. Bryson and W. F. Denham, "A Steepest-Ascent Method for Solving 
%     Optimum Programming Problems," 
%     J. Appl. Mech, vol. 29, no.2, pp 247-257, 1962
%     doi:10.1115/1.3640537
%

% (c) James Whidborne, Cranfield University, 15 March 2016
% revised 19 May 2016
% MATLAB   Version 8.6.0.267246 (R2015b)
% Chebfun  Version 5.3.0  

if (nargin==0),
    aircraft='execujet'
end

%% Define constants for aircraft
switch lower(aircraft)
    
    case {'execujet'}
        %% constants for Executive Jet - ref [5] Appendix B
        % CD = CD0 + K*CL^2     % drag coefficient
        % CL=0.02+0.12*alpha
        
        % conversion factors
        ft2m=0.3048;        % feet per metre
        lb2kg=0.453592;     % lb per kg
        lb2N=4.44822162;    % Newtons per lb force
        
        m=73000*lb2kg;          % mass (kg)
        Thrust_max=2*6250*lb2N; % 2 jet engines with max thrust 6250lb
        S = 950*(ft2m)^2;       % Wing area (m^2)
        AR = 5.9;               % wing aspect ratio
        eta=0.05;               % 1/(pi*osw*AR)
        CD0=0.015;              % CD =0.015 + 0.05 *CL^2; % Minimum drag coefficient
        CLmind = 0;             % Lift at minimum drag
        g = 9.80665;            % gravitational accn (m/s^2)
        rho0 = 1.225;           % air density at mean sea level (kg/m^3)
        W=m*g;                  % weight N
        
        %% Define domain
        Mdomain=[0.1 1.1];                           % Mach range
        hdomain=[0 16000];                           % altitude range (m)
        M = chebfun2(@(M,h) M, [Mdomain hdomain]);    % mach number
        h = chebfun2(@(M,h) h, [Mdomain hdomain]);    % altitude (m)
        
        %% data
        % get temperature, speed of sound Pressure, air density
        [~, a, ~, rho] = isam( h ); % atmosphere (International Standard Atmosphere Model)
        % Thrust chebfun
        Thrust=(rho/rho0).^(0.6)*Thrust_max;% for  Executive Jet (McClamroch book Appendix B)
        
        % convert Mach to airspeed
        V=M.*a;                  % airspeed M=V/a  (m/s^2)
        V2=M.^2.* a.^2;          % airspeed squared
        % lift & drag coefficients
        CL = 2*W./(rho.*V2*S );  % lift coefficient ( L=CL*rho*V^2*S/2 )
        CD = CD0 + eta.*CL.^2;   % drag coefficient
        D=CD.*rho.*V2*S/2;       % drag
        PsCntr=[2:2:20];         % to diaplay excess power contours
        
    case {'aircraft1'}
        %% Bryson, Desai & Hoffman aircraft constants - ref [4,3]
        g = 32.1740 ;               % gravitational accn (ft/s^2)
        S = 530;                    % Wing area (ft^2)
        W=32.0;                     % weight ( 1000 lb)? listed as 42000 in
                                    % paper, and in Betts' book but that
                                    % seems wrong - 32000 gives results
                                    % closer to those in the paper
        %% Define domain
        Mdomain=[0.01 2];                          % Mach range
        hdomain=[0 70];                            % altitude range (1000 ft)
        M = chebfun2(@(M,h) M, [Mdomain hdomain]); % mach number
        h = chebfun2(@(M,h) h, [Mdomain hdomain]); % altitude (1000 ft)
        
        %% data
        if(0)
            % lift drag table data v2 - original from Bryson, Desai &
            % Hoffman, [4] (see also Betts [6])
            M_d =       [0.01,   0.4,  0.8,   0.9,   1.0,   1.2,   1.4,   1.6,   1.8];
            CLalpha_d = [3.44,  3.44,  3.44,  3.58,  4.44,  3.44,  3.01,  2.86,  2.44];
            CD0_d =     [1.3,    1.3,   1.3,  1.4,   3.1,   4.1,   3.9,   3.6,  3.5]/100;
            eta_d =     [0.54,  0.54,  0.54,  0.75,  0.79,  0.78,  0.89,  0.93,  0.93] ;
        else
            % lift drag table data v1 - note: corrections made by jfw
            % based on data in Bryson & Denham, [7]
            M_d =       [0.01,   0.4,   0.6,   0.8,   0.9,   1.0,   1.2,   1.4,   1.6,   1.8];
            CLalpha_d = [3.44,  3.44,  3.44,  3.47,  3.58,  4.44,  3.44,  2.99,  2.66,  2.44];
            CD0_d =     [1.56,  1.50,  1.44,  1.62,  1.93,  3.10,  4.00,  3.62,  3.23,  3.00]/100;
            eta_d =     [0.54,  0.54,  0.54,  0.57,  0.71,  0.77,  0.84,  0.88,  0.91,  0.93] ;%
        end
        
        %   chebfun curve fits
        CLalpha_pchip =chebfun.pchip(M_d,CLalpha_d,Mdomain); % chebfun fit
        CD0_pchip =chebfun.pchip(M_d,CD0_d,Mdomain);         % chebfun fit
        eta_pchip =chebfun.pchip(M_d,eta_d,Mdomain);         % chebfun fit
        
        % now get smooth chebfun2 approximations (chebfun2 must be smooth!)
        CLalpha = chebfuntochebfun2(CLalpha_pchip,hdomain,'y');
        CD0 = chebfuntochebfun2(CD0_pchip,hdomain,'y');
        eta = chebfuntochebfun2(eta_pchip,hdomain,'y');
        
        % get temperature, speed of sound Pressure, air density
        [~, a, ~, rho] = isam( h ,'imperial'); % atmosphere (International Standard Atmosphere Model)
        % thrust table data
        MT_d =[0.01, 0.2:0.2:1.8];
        hT_d=[0, 5, 10, 15, 20, 25, 30, 40, 50, 70]; % 1000 ft
        Thrust_d=... % ( 1000 lb) -- note that numbers have been added to try smooth response when outside FE
            [24.2, 20,  16,    14,   10,   6,   4,      0,     0, 0;
            28.0, 24.6, 21.1, 18.1, 15.2, 12.8, 10.7,   4,     3, 0 ;
            28.3, 25.2, 21.9, 18.7, 15.9, 13.4, 11.2,  7.3,  4.4, 0 ;
            30.8, 27.2, 23.8, 20.5, 17.3, 14.7, 12.3,  8.1,  4.9, 0;
            34.5, 30.3, 26.6, 23.2, 19.8, 16.8, 14.1,  9.4,  5.6, 1.1;
            37.9, 34.3, 30.4, 26.8, 23.3, 19.8, 16.8, 11.2,  6.8, 1.4;
            36.1, 38.0, 34.9, 31.3, 27.3, 23.6, 20.1, 13.4,  8.3, 1.7;
            30,   36.6, 38.5, 36.1, 31.6, 28.1, 24.2, 16.2, 10.0, 2.2;
            25,     30,   35, 38.7, 35.7, 32.0, 28.1, 19.3, 11.9, 2.9;
            20,     25,   30,   35,   34, 34.6, 31.1, 21.7, 13.3, 3.1]';
        
        % Thrust chebfun
        Thrust_interp2 = chebfun2(@(xx,yy) interp2(MT_d,hT_d,Thrust_d,xx,yy, 'spline'),...
            [Mdomain hdomain]); % spline or cubic - note: not smooth
        NM=154; Nh=150; % number of chebychev interpolation points
        [M_pts, h_pts] = chebpts2(NM, Nh, [Mdomain hdomain]); % get interpolation points
        % get values at interpolation points
        Thrust_approx=Thrust_interp2(M_pts, h_pts);
        % now get smooth chebfun2 approximation
        Thrust  = chebfun2(Thrust_approx ,[Mdomain hdomain]);
        % visual check
        %  figure(99), surface(MT_d,hT_d,Thrust_d);hold on ;surface(Thrust); hold off
        
        %% convert Mach to airspeed
        % normalize by dividing by sqrt(1000) because height is in 1000's of feet
        V=M.*a/sqrt(1000);            % airspeed   (ft/s^2/sqrt(1000)) -
        V2=M.^2.* a.^2/1000;          % airspeed squared/1000
        
        %% lift & drag
        % L=(1/2) CL S rho V^2 =W => CL= 2 W/ ( S rho V^2)
        CL = 2*W./(rho.*V2*S ); % lift coefficient ( L=CL*rho*V^2*S/2 )
        % CL= CLalpha(M) alpha => alpha=CL/CLalpha(M)
        alpha=CL./CLalpha;          % calculate alpha
        CD = CD0 + eta.*CL.*alpha.^2;     % drag coefficient
        % CD = CD0 + K*CLalpha*alpha^2;     % drag coefficient
        % D = (1/2) rho V^2 CD S
        % Now CL = 2W/(rho V^2 S)= CLalpha alpha, so
        % D = (1/2) rho V^2 CD0 S + eta alpha W
        D = CD0.*rho.*V2.*S/2 + alpha.*eta.*W;
        PsCntr=[4,6,8,12,16]; % display excess power contours
        
    otherwise
        error(['Undefined aircraft:',aircraft])
end

%% Calculate climb schedule

Es =  g*h + V2/2; % specific energy

% Specific thrust = (Thrust-Drag)/W
Ts=(Thrust-D)/W; % specific thrust

% Specific excess power = (Pthrust-Pdrag)/W = V Ts
Ps=V.*Ts; % specific power

%Optimum climb schedule
optschdl=roots(cross(grad(Es),grad(Ps)));

%% plot result
figure
contour(Es,10,':k','LineWidth', 1.5)       % Specific energy
hold on

[C99,H99]=contour(Ps,PsCntr,'--b','LineWidth',1.5); % Specific excess power
clabel(C99,H99);  % plot contour labels

plot(optschdl,'k', 'LineWidth', 3)    % Optimum climb schedule
FE=roots(Ps);                         % calculate flight envelope
disp(['steady level longitudinal flight ceiling: ', num2str(max(max(imag(FE))))])
plot(FE,'r','LineWidth', 2); % plot flight envelope

xlabel('Mach number $M$','Interpreter' ,'Latex','fontsize',12)
switch lower(aircraft)
    case {'execujet'}
        ylabel('altitude $h$ (m)','Interpreter' ,'Latex','fontsize',12)
    case {'aircraft1'}
        ylabel('altitude $h$ (1000 ft)','Interpreter' ,'Latex','fontsize',12)
end
legend('Specific energy','excess specific thrust',...
    'Optimal climb schedule','Flight envelope',...
    'Location','NorthWest')
%title('Energy height and Specific excess power')
return



%%   function [T, a, P, rho] = isam(h,units)
%
%   chebfuns of International Standard Atmosphere Model
%
%   Inputs :
%   h      : chebfun or chebfun2 of altitude (m) (1000ft)
%   units  : 'metric' or 'imperial' 
%
%   Outputs (metric) (imperial):
%   T      : chebfun or chebfun2 of temperature (K) (Rankine)
%   a      : chebfun or chebfun2 of speed of sound (m/s) (kts)
%   P      : chebfun or chebfun2 of air pressure (P) (lb/ft^2)
%   rho    : chebfun or chebfun2 of air density (kg/m^3) (slug/ft^3)
%


function [T, a, P, rho] = isam( h , units)

%% constants
g= 9.80665;     % gravitational accn (m/s^2)
gamma = 1.4;    % specific heat ratio.
R = 287.0531;   % characteristic gas constant (J/(kg K)) - value from ESDU 77022
Lr =0.0065;     % S lapse rate (K/m)
hts = 11000;    % height of troposphere (m)
T0 = 288.15;    % absolute temperature at mean sea level (K)
P0 = 101325;    % static pressure at mean sea level (P)
rho0 = 1.225;   % air density at mean sea level (kg/m^3)
if nargin==1, units='metric'; end
switch lower(units)
    case {'metric'} 
    case{'imperial'}
        ft1000tom=304.8000; % convert 1000 ft to m
        h=ft1000tom*h;
end
%% calculate chebfun2 of T, a, P, rho
hdomain=get(h, 'domain'); % domain of h chebfun 

if hdomain(end)<=hts ; %  smooth functions
    
    T  = T0 - Lr*h;                                 % temperature
    rho = rho0*(T/T0).^((g/(Lr*R))-1.0) ;           % air density
    P = P0*(T/T0).^(g/(Lr*R));                      % air pressure
    
else%  non-smooth functions
    
    % get chebfuns for non-smooth functions
    if(size(hdomain,2)==2), % h is a chebfun
        T  = T0 - Lr*min(h,hts);                    % temperature
        expon  = min(exp(g./(R*T).*(hts - h)),1);
        rho = rho0*(T/T0).^((g/(Lr*R))-1.0).*expon; % air density
        P = P0*(T/T0).^(g/(Lr*R)).*expon;           % air pressure
    else % h is a chebfun2        
        hrange=hdomain(3:4);                        % range of altitude
        Mrange=hdomain(1:2);                        % range of Mach number
        h1=chebfun(@(h1) h1,hrange);                % altitude
        T1  = T0 - Lr*min(h1,hts);                  % temperature
        expon  = min(exp(g./(R*T1).*(hts - h1)),1);
        rho1 = rho0*(T1/T0).^((g/(Lr*R))-1.0).*expon;%air density
        P1 = P0*(T1/T0).^(g/(Lr*R)).*expon;         % air pressure
        
        % approximate chebfun2 from non smooth chebfun
        % (chebfun2s must be smooth)
        T = chebfuntochebfun2(T1,Mrange,'x'); % so Mach is on x axis
        rho = chebfuntochebfun2(rho1,Mrange,'x');
        P = chebfuntochebfun2(P1,Mrange,'x');
    end
    
end

a = sqrt(T*gamma*R);  % speed of sound

%% units
switch lower(units)
    case {'metric'} 
    case{'imperial'}
        KelvintoRankine=9/4;        % convert Kelvin to Rankine
        T=KelvintoRankine*T;
        mperstoknots=1.943844492;   % convert m/s to kt
        mperstoftpersec=1/0.3048;   % convert m/s to ft/s
        a=mperstoftpersec*a;
        Patolbpersqft=0.020885438;  % convert Pascal to lb/ft^2
        P=Patolbpersqft*P;
        kgpercbcmtoslugspercbft=1.94e-3; %convert kg/m^3 to slug/ft^3
        rho=kgpercbcmtoslugspercbft*rho;
end
return

%%
function [cheb2]=chebfuntochebfun2(cheb, domain2,xy2)
% function to approximate non-smooth chebfun by a smooth chebfun2
% domain2 is the domain in the second dimension
% xy2 is the direction of the second (ie constant) direction

Npts=200;  % number of interpolation points
Nxy2=2;    % number of added points for extension into 2nd domain
Ntrunc=80; % truncation order
chebdomain=get(cheb, 'domain');
method=1;  % method (1 by interpolation, 0 by truncation)

switch lower(xy2)
    case {'x'}% x direction is constant
        xdomain=domain2;
        ydomain=[chebdomain(1) chebdomain(end)];
        if(method)
            % approximate chebfun at some interpolation points
            ychebpts=chebpts(Npts,ydomain); % get interpolation points
            chebapprox=cheb(ychebpts);% get values at interpolation points
            cheb2  = chebfun2(repmat(chebapprox,1,Nxy2),[xdomain,ydomain]);% now get smooth chebfun2 approximation
        else
            % through the coefficients of the chebfun mapped to the chebfun2
            chebsmooth=chebfun(cheb,ydomain, 'trunc', Ntrunc); % smooth N-point chebfun approximant
            coeffs=get(chebsmooth,'coeffs',2); % get coefficients
            cheb2=chebfun2([coeffs,coeffs*0],[xdomain,ydomain],'coeffs'); % map to chebfun2
        end
        
    case {'y'}% y direction is constant
        xdomain=[chebdomain(1) chebdomain(end)];
        ydomain=domain2;
        if(method)
            % approximate chebfun at some interpolation points
            xchebpts=chebpts(Npts,xdomain); % get interpolation points
            chebapprox=cheb(xchebpts);% get values at interpolation points
            cheb2  = chebfun2(repmat(chebapprox',Nxy2,1),[xdomain,ydomain]);% now get smooth chebfun2 approximation
        else
            % through the coefficients of the chebfun mapped to the chebfun2
            chebsmooth=chebfun(cheb,xdomain, 'trunc', Ntrunc); % smooth N-point chebfun approximant
            coeffs=get(chebsmooth,'coeffs',2); % get coefficients
            cheb2=chebfun2([coeffs,coeffs*0]',[xdomain,ydomain],'coeffs'); % map to chebfun2
        end
        
    otherwise
        error('specify x or y for second domain')
end

return