%% Define model specification structure
modeldef.type = 'Symbolic';

% Declare model variables: unkowns
modeldef.x =...                  
  {'W_af' ,'W_es'...                  
  ,'p_ic','T_ic','W_ic'...          
  ,'W_th','Aeff_th'...              
  ,'W_wg','Aeff_wg'...              
  ,'T_af','p_af'...                 
  ,'W_c','T_cout','T_c','p_c'...    
  ,'T_imcr','p_im','T_im'...        
  ,'W_e','T_ti','T_em','p_em'...    
  ,'dh_is'...
  ,'W_twg','T_turb','T_t','p_t'...  
  ,'alpha_th','omega_e'...                     
  ,'W_ig','W_fr'...   
  ,'Tq_c','eta_c','omega_tc'...     
  ,'PSI_c','PI_c'...
  ,'W_t','Tq_t','eta_t','PI_t'...   
  ,'u_wg','wg_pos'...               
  ,'DELTA_theta'...                 
  ,'T_e','T_amb','p_amb' ...
  ,'PSI_th','PI_wg'...  
  ,'PSIli_wg','m_af','m_c','m_ic','T_fwd_flow_ic'...
  ,'m_im','m_em','m_t'...
  ,'PI_cnolim','U_c','PHI_model','W_ccorr'...
  ,'Tq_e_cs','Tq_e_cb','Tq_e_f','Tq_e_p','eta_ign'...
  ,'W_fr','W_i_p','FMEP','S_p','BMEP'...
  ,'C_eta_vol','T_in','eta_vol','W_ac','W_fc','T_tout'...
  ,'Tflow_wg'...
  ... %State derivatives:
  ,'dmdt_af','dTdt_af','dmdt_c','dTdt_c'...             
  ,'dmdt_ic','dTdt_ic','dmdt_im','dTdt_im'...
  ,'dmdt_em','dTdt_em','dmdt_t','dTdt_t'...
  ,'domegadt_tc','dwgdt_pos'};               

% Declare model variables: faults
modeldef.f = {'fiml'...         % Intake manifold leakage
             ,'fyw_af'...       % Fault air mass flow sensor
             ,'fyp_im'...       % Fault intake manifold sensor 
             ,'fyp_ic'...       % Fault pressure sensor after throttle
     };    

% Declare model variables: known variables
modeldef.z = {'y_p_ic' ...      % Pressure after throttle
             ,'y_T_ic' ...      % Temperature after throttle
             ,'y_p_im' ...      % Intake manifold pressure
             ,'y_W_af' ...      % Air mass flow
             ,'y_omega_e' ...   % Engine speed
             ,'y_alpha_th' ...  % Throttle position
             ,'y_u_wg' ...      % Wastegate position
             ,'y_wfc' ...       % Computed total fuel mass flow to cylinders
             ,'y_T_amb' ...     % Ambient temperature
             ,'y_p_amb' ...     % Ambient presssure
    };

%% Declare model parameters
modeldef.parameters = {'H_af','plin_af','H_exhaust','plin_exh'...
    ,'H_intercooler','plin_intercooler','PIli_th','gamma_air'...
    ,'PIli_wg','gamma_exh','R_air','cp_air','V_af'...
    ,'V_c','V_ic','V_im','R_exh','cp_exh'...
    ,'lambda','n_r','r_c','V_D','CEva_cool','AF_s','q_HV'...
    ,'eta_ig_ch','s','PI_bl','aux_dev_fric','B'...
    ,'D_c','K1','K2','fix_gain','R_a','cp_a'...
    ,'eta_cmax','eta_cmin','PI_c_max','T_std'...
    ,'k1','k2','cp_eg','k3','k4','D_t','gamma_eg','tau_wg'...
    ,'h_tot', 'Amax'...
    ,'A_0','A_1','A_2'...
    ,'J_tc','xi_fric_tc','V_em','V_t','c_2','c_3','p_std'...
    ,'eta_otto','C_tq1','C_tq2','xi_aux'...
    ,'a1','a2','a3','a4','a5','Q_c11','Q_c12','Q_c22','eta_tmin','eta_tmax','Cd','PI_cmax'...
    ,'cv_exh','cv_air','tau_th'...
    ,'W_ccorrmax','A_em'...
    ,'K_t','T0','h_ext','c0_em','c1_em','c2_em','D','L','Pr','my_exh','lambda_exh'...
    ,'Cic_1','Cic_2','Cic_3', 'TOL'};  

%% Make symbolic objects of model variables and parameters
syms(modeldef.x{:});
syms(modeldef.f{:});
syms(modeldef.z{:});
syms(modeldef.parameters{:});

%% Declared model functions

syms igngrid(omega_e,p_im)
syms a_eta_t_fun(W_t)
syms b_eta_t_fun(W_t)
syms eta_c_fun(eta_cmax,eta_cmin,W_ccorr,W_ccorrmax,PI_c,PI_cmax,Q_c11,Q_c12,Q_c22)
syms W_ic_fun(p_c,p_ic,plin_intercooler,H_intercooler,T_c)
syms W_af_fun(p_amb,p_af,plin_af,H_af,T_amb)
syms W_es_fun(p_amb,p_t,plin_exh,H_exhaust,T_t)
syms PSI_th_fun(p_im,p_ic,gamma_air,PIli_th)
syms PI_wg_fun(p_t,p_em,gamma_air)
syms PSIli_wg_fun(PI_wg,PIli_wg,gamma_exh)
syms Tflow_wg_fun(p_t,p_em,T_em,T_t)
syms W_ig_fun(W_ac,AF_s,W_fc,q_HV,eta_otto,eta_ig_ch,omega_e)
syms W_c_fun(PI_cnolim,p_af,U_c,D_c,T_af,R_air,PHI_model,fix_gain)
syms PHI_model_fun(K1,K2,PSI_c)
syms PI_c_fun(p_c,p_af)
syms W_t_fun(p_em,k1,PI_t,k2,T_em)
syms Tq_t_fun(gamma_eg,cp_eg,eta_t,W_t,T_em,PI_t,omega_tc)
syms PI_t_fun(p_t,p_em)

% Helper functions
syms max_fun(choice_a,choice_b)
syms min_fun(choice_a,choice_b)

%% Declare model equations

modeldef.rels = {...
    ... %Restriction intercooler:
    W_ic == W_ic_fun(p_c,p_ic,plin_intercooler,H_intercooler,T_c)...
    ...
    ... %Restriction air filter:                                                  
    ,W_af == W_af_fun(p_amb,p_af,plin_af,H_af,T_amb)...
    ...
    ... %Restriction exhaust manifold 
    ,W_es == W_es_fun(p_amb,p_t,plin_exh,H_exhaust,T_t)...
    ...
    ... %Restriction throttle:
    ,PSI_th == PSI_th_fun(p_im,p_ic,gamma_air,PIli_th)...
    ,W_th == (Aeff_th*p_ic)/(sqrt(R_air*T_ic))*PSI_th... 
    ,T_imcr == T_ic...
    ...
    ... %Restriction wastegate:
    ,PI_wg == PI_wg_fun(p_t,p_em,gamma_air)...
    ,PSIli_wg == PSIli_wg_fun(PI_wg,PIli_wg,gamma_exh)...
    ,W_wg == (Aeff_wg*p_em)/(sqrt(R_exh*Tflow_wg))*PSIli_wg...
    ,Tflow_wg == Tflow_wg_fun(p_t,p_em,T_em,T_t)...
    ...
    ... %Control volume after air filter:
    ,p_af == m_af*R_air*T_af/V_af...                                            
    ,dmdt_af == W_af - W_c...
    ,dTdt_af == (W_af*cv_air*(T_amb-T_af)+R_air*(T_amb*W_af-T_af*W_c))/(m_af*cv_air)...
    ,DiffConstraint(dmdt_af,m_af)...
    ,DiffConstraint(dTdt_af,T_af)...
    ...
    ... %Control volume compressor:
    ,p_c == m_c*R_air*T_c/V_c...
    ,dmdt_c == W_c - W_ic...
    ,dTdt_c == (W_c*cv_air*(T_cout-T_c)+R_air*(T_cout*W_c-T_c*W_ic))/(m_c*cv_air)...
    ,DiffConstraint(dmdt_c,m_c)...
    ,DiffConstraint(dTdt_c,T_c)...
    ...
    ... %Control volume intercooler:
    ,p_ic == m_ic*R_air*T_ic/V_ic...                                     
    ,dmdt_ic == W_ic - W_th...
    ,dTdt_ic == (W_ic*cv_air*(T_fwd_flow_ic-T_ic)+R_air*...
        (T_fwd_flow_ic*W_ic-T_ic*W_th))/(m_ic*cv_air)...
    ,DiffConstraint(dmdt_ic,m_ic)...
    ,DiffConstraint(dTdt_ic,T_ic)...
    ...
    ... %Control volume intake manifold:
    ,p_im == m_im*R_air*T_im/V_im...                                     
    ,dmdt_im == (W_th - W_ac) + fiml...
    ,dTdt_im == (W_th*cv_air*(T_imcr-T_im) +R_air*...
        (T_imcr*W_th-T_im*W_ac))/(m_im*cv_air)...
    ,DiffConstraint(dmdt_im,m_im)...
    ,DiffConstraint(dTdt_im,T_im)...
    ...
    ... %Control volume exhaust manifold:
    ,p_em == m_em*R_exh*T_em/V_em...                                     
    ,dmdt_em == W_e - W_twg...
    ,dTdt_em == (W_e*cv_exh*(T_ti-T_em)+R_exh*(T_ti*W_e-T_em*W_twg))/(m_em*cv_exh)...
    ,DiffConstraint(dmdt_em,m_em)...
    ,DiffConstraint(dTdt_em,T_em)...
    ...
    ... %Control volume turbine:
    ,p_t == m_t*R_exh*T_t/V_t...                                         
    ,dmdt_t == W_twg - W_es...
    ,dTdt_t == (W_twg*cv_exh*(T_turb-T_t)+...
        R_exh*(T_turb*W_twg-T_t*W_es))/(m_t*cv_exh)...
    ,DiffConstraint(dmdt_t,m_t)...
    ,DiffConstraint(dTdt_t,T_t)...
    ...
    ... % Intercooler reg temperature model:
    ,T_fwd_flow_ic == max_fun(T_amb, T_c - max_fun(TOL,(T_c-T_amb)*(Cic_1 + (T_amb+T_c)*Cic_2/2+W_ic*Cic_3)))...
    ...
    ... % Throttle actuator: 
    ,Aeff_th == A_0 + A_1*(alpha_th*86/100+4)*pi/180 + A_2*((alpha_th*86/100+4)*pi/180)^2 ... 
    ...
    ... % Mass flow to cylinders:
    ,W_ac == eta_vol*V_D*1/(2*pi)*omega_e*p_im/(n_r*R_air*T_im)...
    ,T_in == T_im - CEva_cool*(1/lambda - 1)...
    ,eta_vol == C_eta_vol*((r_c-(p_em/p_im)^(1/gamma_exh))*T_im)/((r_c-1)*T_in)...
    ,C_eta_vol == a1*(max_fun(p_im,2.5e4)*1e-3)^4 + a2*(max_fun(p_im,2.5e4)*1e-3)^3 + a3*(max_fun(p_im,2.5e4)*1e-3)^2 + a4*(max_fun(p_im,2.5e4)*1e-3) + a5 ...
    ...
    ... % Engine torque:
    ,Tq_e_cs == Tq_e_cb-Tq_e_f-Tq_e_p...
    ,Tq_e_cb == eta_ign*W_ig/(4*pi)...
    ,Tq_e_f == W_fr/(4*pi)...
    ,Tq_e_p == W_i_p/(4*pi)...
    ,W_ig == W_ig_fun(W_ac,AF_s,W_fc,q_HV,eta_otto,eta_ig_ch,omega_e)...              
    ,W_fr == V_D*FMEP...
    ,FMEP == xi_aux*((0.464+0.0072*S_p^(1.8))*PI_bl*10^5+0.0215*BMEP)*(0.075/B)^0.5...
    ,S_p == omega_e*0.5/(2*pi*s)...
    ,BMEP == (C_tq1+C_tq2*p_im)... 
    ,W_i_p == V_D*(p_em-p_im) ...
    ,W_e == W_fc + W_ac...
    ...
    ...% Engine-out temperature
    ,T_e == T0+W_e*K_t...
    ...
    ...% Exhaust temperature drop
    ,T_ti == T_amb+(T_e-T_amb)*exp(-h_tot*A_em/(W_e/4*cp_exh))...
    ...
    ... %Compressor
    ,Tq_c == cp_air*W_c*(T_cout-T_af)/omega_tc...
    ,W_c == W_c_fun(PI_cnolim,p_af,U_c,D_c,T_af,R_air,PHI_model,fix_gain)...
    ,PHI_model == PHI_model_fun(K1,K2,PSI_c)...
    ,PSI_c == (2*cp_air*((PI_c^((gamma_air-1)/gamma_air))-1)*T_af)/(U_c^2)...
    ,PI_c == PI_c_fun(p_c,p_af)...
    ,PI_cnolim == p_c/p_af...
    ,U_c == omega_tc*D_c/2 ...
    ,T_cout == T_af*(1+(PI_c^((gamma_air-1)/gamma_air)-1)/eta_c)...
    ,eta_c == eta_c_fun(eta_cmax,eta_cmin,W_ccorr,W_ccorrmax,PI_c,PI_cmax,Q_c11,Q_c12,Q_c22)... 
    ,W_ccorr == W_c*sqrt(T_af/T_std)/(p_af/p_std)...
    ...
    ... % Turbine:
    ,W_t == W_t_fun(p_em,k1,PI_t,k2,T_em)...
    ,Tq_t == Tq_t_fun(gamma_eg,cp_eg,eta_t,W_t,T_em,PI_t,omega_tc)...
    ,eta_t == max_fun(TOL, min_fun(1-TOL, (a_eta_t_fun(omega_tc)*sqrt(T_em)*W_t/(p_em*1e-3)+b_eta_t_fun(omega_tc))/dh_is))...
    ,dh_is == cp_exh*T_em*(1-PI_t^((gamma_exh-1)/gamma_exh))...
    ,T_tout == T_em*(1-eta_t*(1-PI_t^((gamma_eg-1)/gamma_eg)))...
    ,PI_t == PI_t_fun(p_t,p_em)...
    ...
    ... % Adiabatic mixer:
    ,T_turb == (W_t*T_tout + W_wg*Tflow_wg)/(W_t + W_wg)...
    ,W_twg == W_t + W_wg...
    ...
    ... %Wastegate actuator:
    ,Aeff_wg == Amax*Cd*wg_pos... 
    ,dwgdt_pos == 1/tau_wg*(u_wg-wg_pos)...
    ,DiffConstraint(dwgdt_pos,wg_pos)...
    ...
    ... %ignition efficiency:
    ,eta_ign == 1-(DELTA_theta/100)^2*c_2-(DELTA_theta/100)^3*c_3...   
    ...
    ... %Ignition angle modifier:
    ,DELTA_theta == igngrid(omega_e,p_im)...
    ...
    ... %Turbo-Compressor dynamics:
    ,domegadt_tc == 1/J_tc*(Tq_t-Tq_c-omega_tc*xi_fric_tc)... 
    ,DiffConstraint(domegadt_tc,omega_tc)...
    ...
    ... %Mesurement equations:
    ,y_p_ic == p_ic + fyp_ic        ...% Pressure after throttle
    ,y_T_ic == T_ic                 ...% Temperature after throttle   
    ,y_p_im == p_im + fyp_im        ...% Intake manifold pressure 
    ,y_W_af == W_af + fyw_af        ...% Air mass flow
    ,y_omega_e == omega_e           ...% Engine speed
    ,y_alpha_th == alpha_th         ...% Throttle position
    ,y_u_wg == u_wg                 ...% Wastegate position (actuator)
    ,y_wfc == W_fc                  ...% Computed total fuel mass flow
    ,y_T_amb == T_amb               ...% Ambient temperature
    ,y_p_amb == p_amb               ...% Ambient presssure
    };

%% Construct the model object
LiU_ICE_model = DiagnosisModel(modeldef);
LiU_ICE_model.name = 'LiU_ICE model';

% clear temporary variables from workspace
clear( modeldef.x{:} )
clear( modeldef.f{:} )
clear( modeldef.z{:} )
clear( modeldef.parameters{:} )

clear modeldef

clear igngrid a_eta_t_fun b_eta_t_fun eta_c_fun cyl_temp_out
clear W_ic_fun W_af_fun W_es_fun PSI_th_fun PI_wg_fun PSIli_wg_fun
clear Tflow_wg_fun W_ig_fun W_c_fun PHI_model_fun PI_c_fun
clear X_c_fun W_t_fun Tq_t_fun PI_t_fun reg_temp_fun max_fun min_fun
clear choice_a choice_b

%% Evaluate model using Fault Diagnosis Toolbox

% Model summary
LiU_ICE_model.Lint();

% Plot model
figure(1);clf;
LiU_ICE_model.PlotModel();

% Plot Dulmage-Mendelsohn's decomposition
figure(2);clf;
LiU_ICE_model.PlotDM();