function []=TVESLMdisp(beta_median,beta_std,beta_lbound,beta_ubound,sigma_median,sigma_t_median,sigma_t_lbound,sigma_t_ubound,X,Y,n,m,p,k,T,bex,ar,lambda1,lambda2,lambda3,lambda4,lambda5,gamma,IRFt,const,endo,exo,startdate,enddate,stringdates1,decimaldates1,pref, Ylevel, Psi_median, Psi_lbound, Psi_ubound, sizetraining,PriorExcel)

% before displaying and saving the results, start estimating the evaluation measures for the model

% obtain first a point estimate betatilde of the VAR coefficients
% this is simply the median
betatilde=beta_median;
Btilde=reshape(betatilde,k,n);
% use this estimate to produce predicted values for the model, following (a.8.2)
Ytilde=X*Btilde;
% then produce the corresponding residuals, using (a.8.3)
EPStilde=Y-Ytilde;

% check first whether the model is stationary, using (a.7.2)
[stationary eigmodulus]=bear.checkstable(betatilde,n,p,k);

% Compute then the sum of squared residuals
% compute first the RSS matrix, defined in (a.8.4)
RSS=EPStilde'*EPStilde;
% retain only the diagonal elements to get the vector of RSSi values
rss=diag(RSS);

% Go on calculating R2
% generate Mbar
Mbar=eye(T)-ones(T,T)/T;
% then compute the TSS matrix, defined in (a.8.7)
TSS=Y'*Mbar*Y;
% generate the R2 matrix in (a.8.8)
R2=eye(n)-RSS./TSS;
% retain only the diagonal elements to get the vector of R2 values
r2=diag(R2);

% then calculate the adjusted R2, using (a.8.9)
R2bar=eye(n)-((T-1)/(T-k))*(eye(n)-R2);
% retain only the diagonal elements to get the vector of R2bar values
r2bar=diag(R2bar);

% now start displaying and saving the results

% preliminary task: create and open the txt file used to save the results

filelocation=fullfile(pref.results_path, [pref.results_sub '.txt']);
fid=fopen(filelocation,'wt');

% print toolbox header

fprintf('%s\n','');
fprintf(fid,'%s\n','');

fprintf('%s\n','%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%');
fprintf(fid,'%s\n','%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%');
fprintf('%s\n','%                                                                                %%');
fprintf(fid,'%s\n','%                                                                                %');
fprintf('%s\n','%    BAYESIAN ESTIMATION, ANALYSIS AND REGRESSION (BEAR) TOOLBOX                                         %');
fprintf(fid,'%s\n','%    BAYESIAN ESTIMATION, ANALYSIS AND REGRESSION (BEAR) TOOLBOX                                     %');
fprintf('%s\n','%                                                                                %');
fprintf(fid,'%s\n','%                                                                                %');
fprintf('%s\n','%    This statistical package has been developed by the external developments division of the ECB.       %');
fprintf(fid,'%s\n','%    This statistical package has been developed by the external developments division of the ECB.   %');
fprintf('%s\n','%                                                                                %');
fprintf(fid,'%s\n','%                                                                                %');
fprintf('%s\n','%    Authors:                                                                                %');
fprintf(fid,'%s\n','%    Authors:                                                                                %');
fprintf('%s\n','%    Romain Legrand  (Romain Legrand <b00148883@essec.edu>)                                              %');
fprintf(fid,'%s\n','%    Romain Legrand  (Romain Legrand <b00148883@essec.edu>)                                          %');
fprintf('%s\n','%    Alistair Dieppe (adieppe@worldbank.org)                                                             %');
fprintf(fid,'%s\n','%    Alistair Dieppe (adieppe@worldbank.org)                                                         %');
fprintf('%s\n','%    Björn van Roye  (Bjorn.van_Roye@ecb.europa.eu)                                                      %');
fprintf(fid,'%s\n','%    Björn van Roye  (Bjorn.van_Roye@ecb.europa.eu)                                                  %');
fprintf('%s\n','%                                                                                %');
fprintf(fid,'%s\n','%                                                                                %');
fprintf('%s\n','%    Version 5,0                                                                                %');
fprintf(fid,'%s\n','%    Version 5.0                                                                                %');
fprintf('%s\n','%                                                                                %');
fprintf(fid,'%s\n','%                                                                                %');
fprintf('%s\n','%    The authors are grateful to Paolo Bonomolo, Marta Banbura, Martin Bruns, Fabio Canova,              %');
fprintf(fid,'%s\n','%    The authors are grateful to Paolo Bonomolo, Marta Banbura, Martin Bruns, Fabio Canova,          %');
fprintf('%s\n','%    Matteo Ciccarelli, Marek Jarocinski, Niccolo Battistini, Gabriel Bobeica                            %');
fprintf(fid,'%s\n','%    Matteo Ciccarelli, Marek Jarocinski, Niccolo Battistini, Gabriel Bobeica                        %');
fprintf('%s\n','%    Michele Lenza, Chiara Osbat, Mirela Miescu, Gary Koop, Giorgio Primiceri                            %');
fprintf(fid,'%s\n','%    Michele Lenza, Chiara Osbat, Mirela Miescu, Gary Koop, Giorgio Primiceri,                       %');
fprintf('%s\n','%    Michal Rubaszek, Barbara Rossi, Ben Schumann, Peter Welz, Hugo Vega de la Cruz and Francesca Loria. %');
fprintf(fid,'%s\n','%  Michal Rubaszek, Barbara Rossi, Ben Schumann, Peter Welz, Hugo Vega de la Cruz and Francesca Loria%');
fprintf('%s\n','%    valuable input and advice which contributed to improve the quality of this work.                    %');
fprintf(fid,'%s\n','%  valuable input and advice which contributed to improve the quality of this work.                  %');
fprintf('%s\n','%                                                                                %');
fprintf(fid,'%s\n','%                                                                                %');
fprintf('%s\n','%   These programmes are the responsibilities of the authors and not of the ECB and the Worldbank.       %');
fprintf(fid,'%s\n','%   These programmes are the responsibilities of the authors and not of the ECB and the Worldbank.   %');
fprintf('%s\n','%   Errors and ommissions remain those of the authors.                                                   %');
fprintf(fid,'%s\n','%   Errors and ommissions remain those of the authors.                                               %');
fprintf('%s\n','%                                                                                %');
fprintf(fid,'%s\n','%                                                                                %');
fprintf('%s\n','%    Please do not use or quote this work without permission.                                            %');
fprintf(fid,'%s\n','%    Please do not use or quote this work without permission.                                        %');
fprintf('%s\n','%                                                                                %');
fprintf(fid,'%s\n','%                                                                                %');
fprintf('%s\n','%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%');
fprintf(fid,'%s\n','%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%');
% print then estimation results

fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

toolboxinfo='BEAR toolbox estimates';
fprintf('%s\n',toolboxinfo);
fprintf(fid,'%s\n',toolboxinfo);

time=clock;
datestring=datestr(time);
dateinfo=['Date: ' datestring(1,1:11) '   Time: ' datestring(1,13:17)];
fprintf('%s\n',dateinfo);
fprintf(fid,'%s\n',dateinfo);

fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

VARtypeinfo='Stochastic volatility BVAR';
fprintf('%s\n',VARtypeinfo);
fprintf(fid,'%s\n',VARtypeinfo);

modelinfo='Survey Local Mean Stochastic Volatility VAR';
fprintf('%s\n',modelinfo);
fprintf(fid,'%s\n',modelinfo);

if IRFt==1
SVARinfo='structural decomposition: none';
elseif IRFt==2
SVARinfo='structural decomposition: choleski factorisation';
elseif IRFt==3
SVARinfo='structural decomposition: triangular factorisation';
elseif IRFt==4
SVARinfo='structural decomposition: sign restrictions';
end
fprintf('%s\n',SVARinfo);
fprintf(fid,'%s\n',SVARinfo);

temp='endogenous variables: ';
for ii=1:n
temp=[temp ' ' endo{ii,1} ' '];
end
endoinfo=temp;
fprintf('%s\n',endoinfo);
fprintf(fid,'%s\n',endoinfo);

temp='exogenous variables: ';
if const==0 && m==0
temp=[temp ' none'];
elseif const==1 && m==1
temp=[temp ' constant '];
elseif const==0 && m>0
   for ii=1:m-1
   temp=[temp ' ' exo{ii,1} ' '];
   end
elseif const==1 && m>1
temp=[temp ' constant '];
   for ii=1:m-1
   temp=[temp ' ' exo{ii,1} ' '];
   end
end
exoinfo=temp;
fprintf('%s\n',exoinfo);
fprintf(fid,'%s\n',exoinfo);

sampledateinfo=['estimation sample: ' startdate '-' enddate];
fprintf('%s\n',sampledateinfo);
fprintf(fid,'%s\n',sampledateinfo);

samplelengthinfo=['Size of the Trainingsample: ' num2str(sizetraining)];
fprintf('%s\n',samplelengthinfo);
fprintf(fid,'%s\n',samplelengthinfo);

samplelengthinfo=['sample size (omitting initial conditions and Trainingsample): ' num2str(T)];
fprintf('%s\n',samplelengthinfo);
fprintf(fid,'%s\n',samplelengthinfo);

laginfo=['number of lags included in regression: ' num2str(p)];
fprintf('%s\n',laginfo);
fprintf(fid,'%s\n',laginfo);

if PriorExcel==1
    arprint=[];
    for ii=1:n
        arprint=[arprint num2str(ar(ii,1)) '  '];
    end
 hyperparam2=['autoregressive coefficients (ar):                ' arprint];
else
 hyperparam2=['autoregressive coefficients (ar):                ' num2str(ar(1,1))];
end
fprintf('%s\n',hyperparam2);
fprintf(fid,'%s\n',hyperparam2);

hyperparam3=['overall tightness (lambda1):                    ' num2str(lambda1)];
fprintf('%s\n',hyperparam3);
fprintf(fid,'%s\n',hyperparam3);

hyperparam4=['cross-variable weighting (lambda2):             ' num2str(lambda2)];
fprintf('%s\n',hyperparam4);
fprintf(fid,'%s\n',hyperparam4);

hyperparam5=['lag decay (lambda3):                            ' num2str(lambda3)];
fprintf('%s\n',hyperparam5);
fprintf(fid,'%s\n',hyperparam5);

%hyperparam6=['exogenous variable tightness (lambda4):         ' num2str(lambda4)];
%fprintf('%s\n',hyperparam6);
%fprintf(fid,'%s\n',hyperparam6);

if bex==1
hyperparam7=['block exogeneity shrinkage (lambda5):           ' num2str(lambda5)];
fprintf('%s\n',hyperparam7);
fprintf(fid,'%s\n',hyperparam7);
end

hyperparam8=['AR coefficient of stochastic volatiltiy of VAR residuals(gamma):    ' num2str(gamma)];
fprintf('%s\n',hyperparam8);
fprintf(fid,'%s\n',hyperparam8);

% display coefficient estimates

fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

coeffinfo=['VAR coefficients (beta): posterior estimates'];
fprintf('%s\n',coeffinfo);
fprintf(fid,'%s\n',coeffinfo);

for ii=1:n

fprintf('%s\n','');
fprintf(fid,'%s\n','');
if ii~=1
fprintf('%s\n','');
fprintf(fid,'%s\n','');
end

endoinfo=['Endogenous: ' endo{ii,1}];
fprintf('%s\n',endoinfo);
fprintf(fid,'%s\n',endoinfo);

coeffheader=fprintf('%25s %15s %15s %15s %15s\n','','Median','St.dev','Low.bound','Upp.bound');
coeffheader=fprintf(fid,'%25s %15s %15s %15s %15s\n','','Median','St.dev','Low.bound','Upp.bound');

% handle the endogenous
   for jj=1:n
      for kk=1:p
      values=[beta_median((ii-1)*k+n*(kk-1)+jj,1) beta_std((ii-1)*k+n*(kk-1)+jj,1) beta_lbound((ii-1)*k+n*(kk-1)+jj,1) beta_ubound((ii-1)*k+n*(kk-1)+jj,1)];
      fprintf('%25s %15.3f %15.3f %15.3f %15.3f\n',strcat(endo{jj,1},'(-',int2str(kk),')'),values);
      fprintf(fid,'%25s %15.3f %15.3f %15.3f %15.3f\n',strcat(endo{jj,1},'(-',int2str(kk),')'),values);
      end
   end

% handle the exogenous
   % if there is no constant:
   if const==0
      % if there is no exogenous at all, obvioulsy, don't display anything
      if m==0
      % if there is no constant but some other exogenous, display them
      else
         for jj=1:m
         values=[beta_median(ii*k-m+jj,1) beta_std(ii*k-m+jj,1) beta_lbound(ii*k-m+jj,1) beta_ubound(ii*k-m+jj,1)];
         fprintf('%25s %15.3f %15.3f %15.3f %15.3f\n',exo{jj,1},values);
         fprintf(fid,'%25s %15.3f %15.3f %15.3f %15.3f\n',exo{jj,1},values);
         end
      end
   % if there is a constant
   else
   % display the results related to the constant
         values=[beta_median(ii*k-m+1,1) beta_std(ii*k-m+1,1) beta_lbound(ii*k-m+1,1) beta_ubound(ii*k-m+1,1)];
         fprintf('%25s %15.3f %15.3f %15.3f %15.3f\n','Constant',values);
         fprintf(fid,'%25s %15.3f %15.3f %15.3f %15.3f\n','Constant',values);
      % if there is no other exogenous, stop here
      if m==1
      % if there are other exogenous, display their results
      else
         for jj=1:m-1
         values=[beta_median(ii*k-m+jj+1,1) beta_std(ii*k-m+jj+1,1) beta_lbound(ii*k-m+jj+1,1) beta_ubound(ii*k-m+jj+1,1)];
         fprintf('%25s %15.3f %15.3f %15.3f %15.3f\n',exo{jj,1},values);
         fprintf(fid,'%25s %15.3f %15.3f %15.3f %15.3f\n',exo{jj,1},values);
         end
      end
   end

fprintf('%s\n','');
fprintf(fid,'%s\n','');

% display evaluation measures
rssinfo=['Sum of squared residuals: ' num2str(rss(ii,1),'%.2f')];
fprintf('%s\n',rssinfo);
fprintf(fid,'%s\n',rssinfo);

r2info=['R-squared: ' num2str(r2(ii,1),'%.3f')];
fprintf('%s\n',r2info);
fprintf(fid,'%s\n',r2info);

adjr2info=['adj. R-squared: ' num2str(r2bar(ii,1),'%.3f')];
fprintf('%s\n',adjr2info);
fprintf(fid,'%s\n',adjr2info);

end

fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

% display VAR stability results
eigmodulus=reshape(eigmodulus,p,n);
stabilityinfo1=['Roots of the characteristic polynomial (modulus):'];
fprintf('%s\n',stabilityinfo1);
fprintf(fid,'%s\n',stabilityinfo1);
for ii=1:p
temp=num2str(eigmodulus(ii,1),'%.3f');
   for jj=2:n
   temp=[temp,'  ',num2str(eigmodulus(ii,jj),'%.3f')];
   end
fprintf('%s\n',temp);
fprintf(fid,'%s\n',temp);
end
if stationary==1;
stabilityinfo2=['No root lies outside the unit circle.'];
stabilityinfo3=['The estimated VAR model satisfies the stability condition'];
fprintf('%s\n',stabilityinfo2);
fprintf(fid,'%s\n',stabilityinfo2);
fprintf('%s\n',stabilityinfo3);
fprintf(fid,'%s\n',stabilityinfo3);
else
stabilityinfo2=['Warning: at leat one root lies on or outside the unit circle.'];
stabilityinfo3=['The estimated VAR model will not be stable'];
fprintf('%s\n',stabilityinfo2);
fprintf(fid,'%s\n',stabilityinfo2);
fprintf('%s\n',stabilityinfo3);
fprintf(fid,'%s\n',stabilityinfo3);
end

fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

% display posterior for sigma
sigmainfo1=['sigma (residual covariance matrix): posterior estimates'];
fprintf('%s\n',sigmainfo1);
fprintf(fid,'%s\n',sigmainfo1);
% calculate the (integer) length of the largest number in sigma, for formatting purpose
width=length(sprintf('%d',floor(max(abs(bear.vec(sigma_median))))));
% add a separator, a potential minus sign, and three digits (total=5) to obtain the total space for each entry in the matrix
width=width+5;
for ii=1:n
temp=[];
   for jj=1:n
   % convert matrix entry into string
   number=num2str(sigma_median(ii,jj),'% .3f');
      % pad potential missing blanks
      while numel(number)<width
      number=[' ' number];
      end
   number=[number '  '];
   temp=[temp number];
   end
fprintf('%s\n',temp);
fprintf(fid,'%s\n',temp);
end
% notice about use of long-run values
sigmainfo2=['Note: Last period observation variance covariance matrix'];
sigmainfo3=['Note: The last period observation variance covariance matrix will also be used for identification.'];
%sigmainfo4=['Note: The historical decomposition and structural shocks are dependend on this choice.'];

fprintf('%s\n',sigmainfo2);
fprintf(fid,'%s\n',sigmainfo2);
fprintf('%s\n',sigmainfo3);
fprintf(fid,'%s\n',sigmainfo3);
%fprintf('%s\n',sigmainfo4);
%fprintf(fid,'%s\n',sigmainfo4);
fclose(fid);

% Finally, display the results in terms of graph
if pref.plot
% plot actual vs. fitted
actualfitted=figure('Tag','BEARresults');
set(actualfitted,'Color',[0.9 0.9 0.9]);
set(actualfitted,'name','model estimation: actual vs fitted')
ncolumns=ceil(n^0.5);
nrows=ceil(n/ncolumns);
for ii=1:n
subplot(nrows,ncolumns,ii)
hold on
plot(decimaldates1,Y(:,ii),'Color',[0 0 0],'LineWidth',2);
plot(decimaldates1,Ytilde(:,ii),'Color',[1 0 0],'LineWidth',2);
hold off
set(gca,'XLim',[decimaldates1(1,1) decimaldates1(end,1)],'FontName','Times New Roman');
title(endo{ii,1},'FontName','Times New Roman','FontSize',10,'FontWeight','normal');
   if ii==1
   plotlegend=legend('actual','fitted');
   set(plotlegend,'FontName','Times New Roman');
   end
end

% plot the residuals
residuals=figure('Tag','BEARresults');
set(residuals,'Color',[0.9 0.9 0.9]);
set(residuals,'name','model estimation: residuals')
for ii=1:n
subplot(nrows,ncolumns,ii)
plot(decimaldates1,EPStilde(:,ii),'Color',[0 0 0],'LineWidth',2)
set(gca,'XLim',[decimaldates1(1,1) decimaldates1(end,1)],'FontName','Times New Roman');
title(endo{ii,1},'FontName','Times New Roman','FontSize',10,'FontWeight','normal');
end

% plot first the time-varying variance and covariance estimates
varcov=figure('Tag','BEARresults');
set(varcov,'Color',[0.9 0.9 0.9]);
set(varcov,'name','model estimation: residual variance and covariance')
for ii=1:n
   for jj=1:ii
   subplot(n,n,n*(ii-1)+jj)
   hold on
   Xpatch=[decimaldates1' fliplr(decimaldates1')];
   Ypatch=[sigma_t_lbound{ii,jj}(p+1:end,1)' fliplr(sigma_t_ubound{ii,jj}(p+1:end,1)')];
   HDpatch=patch(Xpatch,Ypatch,[0.7 0.78 1]);
   set(HDpatch,'facealpha',0.6);
   set(HDpatch,'edgecolor','none');
   plot(decimaldates1,sigma_t_median{ii,jj}(p+1:end,1),'Color',[0.4 0.4 1],'LineWidth',2);
   plot([decimaldates1(1,1),decimaldates1(end,1)],[0 0],'k--');
   hold off
   set(gca,'XLim',[decimaldates1(1,1) decimaldates1(end,1)],'FontName','Times New Roman');
      % top labels
      if jj==ii
      title(['var(' endo{ii,1} ')'],'FontWeight','normal');
      else
      title(['cov(' endo{jj,1} ',' endo{ii,1} ')'],'FontWeight','normal');
      end
   end
end

end % pref.plot

% finally, save the results on excel

% compute the cell for actual and fitted
% create the cell that will be saved on excel
afcell={};
% build preliminary elements: space between the tables
vertspace=repmat({''},T+3,1);
% loop over variables (horizontal dimension)
for ii=1:n
% create cell of actual/fitted for variable ii
temp=['actual and fitted: ' endo{ii,1}];
af_i=[temp {''} {''};{''} {''} {''};{''} {'sample'} {'fitted'};stringdates1 num2cell(Y(:,ii)) num2cell(Ytilde(:,ii))];
afcell=[afcell af_i vertspace];
end
% trim
afcell=afcell(:,1:end-1);
% write in excel
if pref.results==1
    bear.xlswritegeneral(fullfile(pref.results_path, [pref.results_sub '.xlsx']),afcell,'actual fitted','B2');
end

% then compute the cell for the residuals
% create the cell that will be saved on excel
horzspace=repmat({''},1,n);
rescell=[{'residuals'} horzspace;{''} horzspace;{''} endo';stringdates1 num2cell(EPStilde)];
% write in excel
if pref.results==1
    bear.xlswritegeneral(fullfile(pref.results_path, [pref.results_sub '.xlsx']),rescell,'resids','B2');
end

% finally compute the cell for the time varying variance and covariance
% create the cell that will be saved on excel
varcovcell={};
% build preliminary elements: space between the tables
horzspace=repmat({''},2,5*n);
vertspace=repmat({''},T+3,1);
% loop over variables (vertical dimension)
for ii=1:n
tempcell={};
   % loop over shocks (horizontal dimension)
   for jj=1:ii
   % create cell of hd record for the contribution of shock jj in variable ii fluctuation
      % if a sign restriction identification scheme has been used, use the structural shock labels
      if jj==ii
      temp=['variance of ' endo{ii,1} ' residuals'];
      % otherwise, the shocks are just orthogonalised shocks from the variables: use variable names
      else
      temp=['covariance between ' endo{jj,1} ' and ' endo{ii,1} 'residuals'];
      end
   vc_ij=[temp {''} {''} {''};{''} {''} {''} {''};{''} {'lw. bound'} {'median'} {'up. bound'};stringdates1 num2cell(sigma_t_lbound{ii,jj}(p+1:end,:)) num2cell(sigma_t_median{ii,jj}(p+1:end,:)) num2cell(sigma_t_ubound{ii,jj}(p+1:end,:))];
   tempcell=[tempcell vc_ij vertspace];
   end
   % complete with blanks (for repeated covariance values)
   for jj=1:n-ii
   tempcell=[tempcell cell(T+3,5)];
   end
varcovcell=[varcovcell;horzspace;tempcell];
end
% trim
varcovcell=varcovcell(3:end,1:end-1);
% write in excel
if pref.results==1
    bear.xlswritegeneral(fullfile(pref.results_path, [pref.results_sub '.xlsx']),varcovcell,'time variation','B2');
end
%trend and cycle decomposition
%plot
% Finally, display the results in terms of graph
if pref.plot
% plot actual vs. fitted
trendcycle=figure('Tag','BEARresults');
set(trendcycle,'Color',[0.9 0.9 0.9]);
set(trendcycle,'name','Local mean (trend) and actual data')
ncolumns=ceil(n^0.5);
nrows=ceil(n/ncolumns);
for ii=1:n
subplot(nrows,ncolumns,ii)
hold on
Xpatch=[decimaldates1' fliplr(decimaldates1')];
Ypatch=[Psi_lbound(p+1:end,ii)' fliplr(Psi_ubound(p+1:end,ii)')];
HDpatch=patch(Xpatch,Ypatch,[0.7 0.78 1]);
set(HDpatch,'facealpha',0.6);
set(HDpatch,'edgecolor','none');
plot(decimaldates1,Psi_median(p+1:end,ii),'Color',[1 0 0],'LineWidth',2);
plot(decimaldates1,Ylevel(p+1:end,ii),'Color',[0 0 0],'LineWidth',2);
hold off
set(gca,'XLim',[decimaldates1(1,1) decimaldates1(end,1)],'FontName','Times New Roman');
title(endo{ii,1},'FontName','Times New Roman','FontSize',10,'FontWeight','normal');
   if ii==1
   plotlegend=legend('Credible Bands Local Mean', 'Median Estimate Local Mean (Trend component)','Actual Data');
   set(plotlegend,'FontName','Times New Roman');
   % user defined position (ex : right bottom)
   set(plotlegend,'Position',[0.7443    0.1782    0.1518    0.1286])
   % automatic
   %set(plotlegend,'Location','best')
   end
end

%finally record the local mean estimates in excel

% compute the cell for actual and fitted
% create the cell that will be saved on excel
trendcell={};
% build preliminary elements: space between the tables
vertspace=repmat({''},T+3,1);
% loop over variables (horizontal dimension)
for ii=1:n
% create cell of actual/fitted for variable ii
temp=['Local mean estimates: ' endo{ii,1}];
trendcell_i=[temp {''} {''} {''};{''} {''} {''} {''};{'dates'} {'upper bound'} {'median'} {'lower bound'};stringdates1 num2cell(Psi_ubound(p+1:end,ii)) num2cell(Psi_median(p+1:end,ii)) num2cell(Psi_lbound(p+1:end,ii))];
trendcell=[trendcell trendcell_i vertspace];
end
% trim
trendcell=trendcell(:,1:end-1);
% write in excel
if pref.results==1
    bear.xlswritegeneral(fullfile(pref.results_path, [pref.results_sub '.xlsx']),trendcell,'Local Mean Estimates','B2');
end

end


function new_x = monolist(x,dmax,dmin)
% MONOLIST Generate monomials
%
% y = MONOLIST(x,dmax,dmin)
%
% Returns the monomials [1 x(1) x(1)^2 ... x(1)^dmax(1) x(2) x(1)x(2)  etc...]
%
% >>sdpvar x y z
% >>sdisplay(monolist([x y z],4))
%
%  Input
%     x      : Vector with SDPVAR variables
%     dmax   : Integers > 0
%
%  See also POLYNOMIAL, DEGREE

% Flatten
x = reshape(x,1,length(x));
x_orig = x;

if nargin == 3
    if length(dmin)>1 || any(dmin > dmax) || ~isequal(dmin,fix(dmin))
        error('dmin has to be an integer scalar larger than dmax');
    end
elseif nargin == 2
    dmin = 0;
end

if (length(dmax)==1 | all(dmax(1)==dmax)) & islinear(x) & ~isa(x,'ncvar')
    dmax = dmax(1);
    % Powers in monomials
    powers = monpowers(length(x),dmax);

    powers = powers(find(sum(powers,2)>=dmin),:);

    % Use fast method for x^alpha
    if isequal(getbase(x),[zeros(length(x),1) eye(length(x))])
        new_x = recovermonoms(powers,x);
        return
    end

    % Monolist on dense vectors is currently extremely slow, but also
    % needed in some applications (stability analysis using SOS) For
    % performance issue, the code below is hard-coded for special cases
    % FIX : Urgent, find underlying indexing...

    % Vectorize quadratic and quadrtic case
    if dmax==2 & length(x)>1
        V=x.'*[1 x];
        ind=funkyindicies(length(x));
        new_x = [1 V(ind(:)).'].';
        return
    elseif (length(x)==4 & dmax==6)

         ind =[    1           2           3           4           5           6           7           8,

           9          10          11          12          13          14          15          16,

          17          18          19          20          21          22          23          24,

          25          26          27          28          29          30          31          32,

          33          34          49          50          51          52          86          53,

          54          55          89          56          57          91          58          92,

         126          59          60          61          95          62          63          97,

          64          98         132          65          66         100          67         101,

         135          68         102         136         170         185         186         187,

         188         222         256         189         190         191         225         259,

         192         193         227         261         194         228         262         296,

         330         364         195         196         197         231         265         198,

         199         233         267         200         234         268         302         336,

         370         201         202         236         270         203         237         271,

         305         339         373         204         238         272         306         340,

         374         408         442         476         510         525         526         527,

         528         562         596         630         529         530         531         565,

         599         633         532         533         567         601         635         534,

         568         602         636         670         704         738         772         806,

         840         535         536         537         571         605         639         538,

         539         573         607         641         540         574         608         642,

         676         710         744         778         812         846         541         542,

         576         610         644         543         577         611         645         679,

         713         747         781         815         849         544         578         612,

         646         680         714         748         782         816         850         884,

         918         952         986        1020        1054        1088        1122        1156,

        1190          0            0          0          0             0           0            0];
        ind = ind';
        ind = ind(find(ind));
        v=monolist(x,3);
        V=v(2:end)*v.';
        new_x = [1;V(ind(:))];
        return
    elseif dmax==4 & (1<=length(x)) & length(x)<=4 %& length(x)>1
        v=monolist(x,2);
        V=v(2:end)*v.';

        % Cone to generate indicies
        %p = sdpvar(n,1);
        %v = monolist(p,2);
        %V = v(2:end)*v';V=V(:);
        %m = monolist(p,4)
        %ind = [];
        %for i = 2:length(m)
        % ind = [ind min(find(~any(V-m(i))))];
        %end

        switch length(x)
            case 1
                new_x = [1; V([1 2 4 6]')];
                return
            case 2
                new_x = [1;V([1 2 3 4 5 8 9 10 15 18 19 20 25 30]')];
                return;
            case 3
                new_x=[1;V([1 2 3 4 5 6 7 8 9 13 14 15 24 16 17 26 18 27 36 40 41 42 51 60 43 44 53 62 45 54 63 72 81 90]')];
                return
            case 4
                new_x=[1;V([    1     2     3     4     5     6     7     8     9    10    11    12    13    14    19    20    21    35    22    23    37    24    38    52    25    26    40    27    41    55    28    42 56    70    75    76    77    91   105    78    79    93   107    80    94   108   122   136  150    81    82    96   110    83    97   111   125   139   153    84    98   112   126   140  154   168   182   196   210]')];
                return
            otherwise
        end
    end

    % Na, we have things like (c'x)^alpha
    % precalc x^p
    for i = 1:length(x)
        temp = x(i);
        precalc{i,1} = temp;
        for j = 2:1:dmax
            temp = temp*x(i);
            precalc{i,j} = temp;
        end
    end

    new_x = [];

    for i = 1:size(powers,1) % All monomials
        temp = 1;

        for j = 1:size(powers,2) % All variables
            if powers(i,j)>0
                temp = temp*precalc{j,powers(i,j)};
            end
        end
        new_x = [new_x temp];

    end

else

    dmax = dmax(:)*ones(1,length(x_orig));

    x = [1 x];

    % Lame loop to generate all combinations
    new_x = 1;
    for j = 1:1:max(dmax)
        temp = [];
        for i = 1:length(x)
            temp = [temp x(i)*new_x];
        end
        new_x = temp;
        new_x = fliplr(unique(new_x));
        new_degrees = degree(new_x,x(2:end));
        remv = [];
        for i = 1:length(dmax);
            if new_degrees(i)>=dmax(i)
                x = recover(setdiff(getvariables(x),getvariables(x_orig(i))));
                x = [1;x(:)];
                remv = [remv i];
            end
        end
        dmax = dmax(setdiff(1:length(dmax),remv));
    end
end
new_x = reshape(new_x(:),length(new_x),1);
if dmin > 0
    for i = 1:length(new_x)
        if sum(powers(i,:)) < dmin
            keep(i) = 0;
        else
            keep(i) = 1;
        end
    end
    if any(keep==0)
        new_x = new_x(find(keep));
    end
end

function ind = funkyindicies(n)

M=reshape(1:n*(n+1),n,n+1);
ind = M(:,1)';
for i = 1:n
    ind = [ind M(i,2:i+1)];
end


% calc_frap - makes the actual analysis of the FRAP data
%   [k,D1k,D1,D2,gamma2,gamma0,v_conv,a_conv]=...
%   calc_frap(file_in,dx,Ipre,Idark,istart,t,trc,Rp_max,nr) where:
%
%   k =  spatial frequencies [um^-1]
%   D1k = D1(k2), the diffusion coefficient determined for each k
%       separately [um^2s^-1]
%   D1 and D2 = effective values of the diffusion coefficients D1 and
%       D2 [um^2s^-1]
%   gamma2 = the intensity fraction of component 2
%   gamma0 = the intensity fraction of immobile molecules
%   v_conv = the absolute velocity of the tracked center of mass [um/s]
%   a_conv = the angle the tracked center of mass moves in, with 0 degrees
%       corresponding to moving horizontally from the left to the right
%       [degrees]
%
%   file_in = the filename + directory of the image stack
%   dx = the pixel size (square pixels assumed)
%   Ipre = the pre-bleach intensity
%   Idark = the dark count intensity
%   istart = the first post-bleach frame
%   t = the times for each frame
%   trc = parameter that if equal to 'y' leads to tracking of the center of
%       mass for each frame (otherwise the center of mass is determined from
%       the first post-bleach frame)
%   Rp_max = the maximum radial value used in the analysis [pixels]
%   nr = a vector for all frames in the image stack. If nr==1 then this
%       image will be part of the analysis.

function [k,D1k,D1,D2,gamma2,gamma0,v_conv,a_conv]=...
    calc_frap(file_in,dx,Ipre,Idark,istart,t,trc,Rp_max,nr)

% closes the figures
close(figure(1))
close(figure(2))

% Meshpoints
x=(1:1:size(Ipre,2));
y=(size(Ipre,1):-1:1);
[Xx,Yy]=meshgrid(x,y);

% Calculates the centre of mass from the first frame after bleaching
I=double(imread(file_in,istart))-Idark;
I2=I./Ipre;

fig=figure(1);
set(fig,'Color','w');
I2_min=max(0.5,min(I2(:)));
imshow(I2,[I2_min,min([1+(1-I2_min)*0.1,max(I2(:))])]);
ax1 = gca;
set(ax1,'FontSize',8)
axis square
h=title({'Define a polygon around the bleached spot (close with the right mouse button).';...
    'Exit by double clicking the inner part of the polygon.'},...
    'Color','k','BackgroundColor','w');
hp=get(h);
pos=hp.Position;
pos(2)=pos(2)+50;
set(h,'Position',pos);
BW=roipoly; % the region in which the bleached spot is situated

% Calculates the centre of mass (x_cm,y_cm)
x_cm=sum(sum(Xx.*(1-I2).*BW))/sum(sum((1-I2).*BW));
y_cm=sum(sum(Yy.*(1-I2).*BW))/sum(sum((1-I2).*BW));
Rr=sqrt((Xx-x_cm).^2+(Yy-y_cm).^2);
wid=max(Rr(BW==1)); % max r-value to use to calculate the center of mass

figure(1)
hold on
plot3([x_cm-10,x_cm+10],[max(Yy(:))-y_cm+1,max(Yy(:))-y_cm+1],max(I2(:))*ones(1,2),...
    [x_cm,x_cm],[max(Yy(:))-y_cm+1-10,max(Yy(:))-y_cm+1+10],max(I2(:))*ones(1,2),'LineWidth',2,'Color',[1 1 1])
hold off
pause(0.2)

% Defines the options for nonlinear fitting
options=optimset('Display','off');

Nrp=512;    % the length of r

% Determines the radial distance r and the radius of the field of view R
[test,r,R,Rp_max]=I_radial(Nrp,x_cm,y_cm,Xx,Yy,Xx,Rp_max);
r=r*dx;
R=R*dx;

% Initiates variables
wf=zeros(sum(nr==1),1);
x_cm2=zeros(sum(nr==1),1);
y_cm2=zeros(sum(nr==1),1);
Ixx=zeros(size(x_cm2));
Iyy=zeros(size(x_cm2));
Ixy=zeros(size(x_cm2));
Af=zeros(sum(nr==1),1);
Isum=zeros(sum(nr==1),1);
Ir=zeros(sum(nr==1),Nrp);
g=zeros(sum(nr==1),Nrp);
i=0;

t=t(nr==1)';    % updates the time vector

for n_t=1:length(nr)
    % n_t==0 if a frame is omitted from the analysis
    if nr(n_t)==1
        i=i+1;

        I=double(imread(file_in,istart+n_t-1))-Idark;   % Reads the image
        I2=I./Ipre; % The relative intensity

        if i==1
            x_cm2(i)=x_cm;
            y_cm2(i)=y_cm;

            I20=I2;
        else
            x_cm2(i)=x_cm2(i-1);
            y_cm2(i)=y_cm2(i-1);
        end

        if i>2
            x_cm2(i)=(x_cm2(i-1)-x_cm2(i-2))/(t(i-1)-t(i-2))*(t(i)-t(i-1))+x_cm2(i-1);
            y_cm2(i)=(y_cm2(i-1)-y_cm2(i-2))/(t(i-1)-t(i-2))*(t(i)-t(i-1))+y_cm2(i-1);
        end

%         % Take away the immobile fraction
%         if trc=='y'
%             gamma_g=0.020;   % immobile fraction
%             I2=I2+(I2_inf-I20)*gamma_g;
%         end

        % Determines I2(inf) using values at the edge of the image
        nx=round(length(x)*0.1);
        ny=round(length(y)*0.1);
        I2s=sum(sum(I2(1:ny,:)))+sum(sum(I2(end:end-ny,:)))+...
            sum(sum(I2(:,1:nx)))+sum(sum(I2(:,end:end-nx)));
        I2n=sum(sum(~isnan(I2(1:ny,:))))+sum(sum(~isnan(I2(end:end-ny,:))))+...
            sum(sum(~isnan(I2(:,1:nx))))+sum(sum(~isnan(I2(:,end:end-nx))));
        I2_inf=I2s/I2n;
        beta=I2_inf;

        % If trc=='y' then the program tracks (and moves) the center of mass
        % for each frame
        Iin=(I2_inf-I2);
        if trc=='y'
            for j=1:3
                Rr2=(Xx-x_cm2(i)).^2+(Yy-y_cm2(i)).^2;
                BW=Rr2<(wid^2);
                cm_w=Iin(BW==1);
                Xx_w=Xx(BW==1);
                Yy_w=Yy(BW==1);

                % Calculates the centre of mass (x_cm,y_cm)
                x_cm2(i)=sum(Xx_w.*cm_w)/sum(cm_w);
                y_cm2(i)=sum(Yy_w.*cm_w)/sum(cm_w);
            end
        else
            Rr2=(Xx-x_cm2(i)).^2+(Yy-y_cm2(i)).^2;
            BW=Rr2<(wid^2);
            cm_w=Iin(BW==1);
            Xx_w=Xx(BW==1);
            Yy_w=Yy(BW==1);
            x_cm2(i)=x_cm;
            y_cm2(i)=y_cm;
        end

        % Calculates the moments (Ixx and Iyy) and product (Ixy) of inertia for the bleached spot
        Ixx(i)=sum((Yy_w-y_cm2(i)).^2.*cm_w);
        Iyy(i)=sum((Xx_w-x_cm2(i)).^2.*cm_w);
        Ixy(i)=sum((Xx_w-x_cm2(i)).*(Yy_w-y_cm2(i)).*cm_w);

        % Performs the radial averaging of I2 around the centre of mass
        Ir(i,:)=I_radial(Nrp,x_cm2(i),y_cm2(i),Xx,Yy,I2,Rp_max);

        % Determines Ir(R,0) using values where r>0.9R
        if i==1
            Ir0=mean(Ir(1,r>(max(r)*0.9)));
        end

        % Determines the integral of Ir at r<R
        Isum(i)=2*pi*Ir(i,1)*r(1)*r(1);
        for j=2:length(r)
            Isum(i)=Isum(i)+2*pi*(Ir(i,j)*r(j)+Ir(i,j-1)*r(j-1))/2*(r(j)-r(j-1));
        end

        % Determines starting parameters used to fit the tail of Ir
        y=Ir(i,:)/Ir0;
        x=r;
        pos=1;
        y0=y(pos);
        while (1-y0)>(1-y(1))*exp(-1) && pos<length(y)
            pos=pos+1;
            y0=y(pos);
        end
        w_g0=sqrt(x(pos)^2-x(1)^2);

        y=Ir(i,r>max(r)*1/2)/Ir0;
        x=r(r>max(r)*1/2);
        pos=1;
        y0=y(pos);
        while (1-y0)>(1-y(1))*exp(-1) && pos<length(y)
            pos=pos+1;
            y0=y(pos);
        end
        w_g=max(sqrt(x(pos)^2-x(1)^2),w_g0);

        % Makes a curve fit to the tail of Ir (for r>R/2) for each frame
        p0=[min(1-Ir(i,1)/Ir0,abs(1-y(1))*exp(x(1)^2/w_g^2)),w_g];
        p0_2=p0;
        if i>1
            p0_2=p;
        end

        [p1,sse1]=lsqcurvefit(@(p,r) fkn_gauss(p,r,Isum(i),Isum(1),Ir0),p0,x,y*Ir0,...
            [0,0],[Inf,Inf],options);

        [p2,sse2]=lsqcurvefit(@(p,r) fkn_gauss(p,r,Isum(i),Isum(1),Ir0),p0_2,x,y*Ir0,...
            [0,0],[Inf,Inf],options);

        if sse1<sse2
            p=p1;
        else
            p=p2;
        end

        wf(i)=p(2); % w(t)
        Af(i)=p(1); % A(t)
        [y,beta]=fkn_gauss(p,r,Isum(i),Isum(1),Ir0);    % y = Ir|r>R

        % Compensates for temporal variations using beta and for
        % a net influx of molecules with y
        g(i,:)=(y-Ir(i,:))/beta;

        if mod(i,10)==0 || i==1 || i==sum(nr==1)
            figure(2)
            ax1 = gca;
            set(ax1,'FontSize',12)
            plot(r,Ir(i,:)/beta,'b.',r(r>(max(r)*1/2)),y(r>(max(r)*1/2))/beta,'r-','LineWidth',2)
            xlabel('r [\mum]','FontSize',12)
            ylabel('Intensity [a.u.]','FontSize',12)
            title(['Frame ',num2str(i),' out of ',num2str(sum(nr==1))],'FontSize',12)
            xlim([min(r),max(r)])
            pause(0.1)
        end

    end
end

% Calculates the Hankel transform and performs the curve fits yielding D1,
% D2, gamma2 and gamma0
[k,D1k,D1,D2,gamma2,gamma0,v_conv,a_conv]=...
    hankel_diff(r,t,R,g,wf,Af,Nrp,trc,x_cm2,y_cm2,dx,Ixx,Iyy,Ixy);

end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% Tutorial for FAIR: compact version of matrix-based regularization
%
%  S(y) = alpha/2 * norm(B*(y-yRef))^2,
%
% where
%  alpha regularization parameter, weights regularization versus
%        distance in the joint objective function, alpha = 1 here
%  yRef  is a reference configuration, e.g. yRef = x or a
%        pre-registration result
%  B     a discrete partial differential operator either in explicit
%        matrix form or as a structure containing the necessary
%        parameters to compute B*y
% see also regularizer E8_regularization_MF
%==============================================================================

clear, close all, help(mfilename);

% (c) Jan Modersitzki 2009/03/25, see FAIR.2 and FAIRcopyright.m.
% note: '%' is used so hide comments in the book
% Example for usage of regularization
% (c) Jan Modersitzki 2009/04/02, see FAIR.2 and FAIRcopyright.m.
% illustrates the usage of L2-norm based regularization
%
%

% initialize the regularization and create a starting  point
regularizer('reset','regularizer','mbElastic','alpha',1,'mu',1,'lambda',0);
y0 = @(omega,m) randn(size(getStaggeredGrid(omega,m)));

% 2D example, initialize physical domain and number of discretization points
omega = [0,1,0,1]; m = [16,12];   %

% test derivative of 2D implementation
fctn = @(yc) regularizer(yc,omega,m);  checkDerivative(fctn,y0(omega,m));

% 3D example, initialize physical domain and number of discretization points
omega = [0,1,0,1,0,1]; m  = [16,12,8];

% test derivative of 3D implementation
fctn = @(yc) regularizer(yc,omega,m);
checkDerivative(fctn,y0(omega,m));
%==============================================================================


%-------------------------------------------------------------------------
% Coupling Kinetic and Fluid System of Euler Equations
% To solve Shock Tube Problem
%
% Kinetic equation use SBBGK and Hydrodynamic equation use Roe Euler
%
% Based on:
%  [1] Pierre Degond, Giacomo Dimarco and Luc Mieussens
%      A moving interface method for dynamic kinetic-fluid coupling
%      Journal of Computation Physics 227(2007)1176-1208
%
% By Manuel Diaz and Yun-Da Tsai
% 007@IAM 25.01.2013
%-------------------------------------------------------------------------

clear all; clc; close all;

%% Global Variables
global CFL r_time theta dt dtdx nx
global w k nv
global gamma etpfix

%% Controling Parameters
name        ='SBBGK1d'; % Simulation Name
CFL         = 0.05;     % CFL condition
r_time      = 1/10000;  % Relaxation time
tEnd        = 0.1;      % End time
theta       = 0;        % {-1} BE, {0} MB, {1} FD.
quad        = 2;        % for NC = 1 , GH = 2
method      = 1;        % for TVD = 1, WENO3 = 2, WENO5 = 3
IC_case     = 1;        % IC: {1}Sod's, {2}LE, {3}RE, {4}DS, {5}SS, {6}Cavitation
plot_figs   = 1;        % 0: no, 1: yes please!
write_ans   = 0;        % 0: no, 1: yes please!
gamma       = 2.4;      % Ratio of specific heats
flxtype     = 2;        % {1} Roe, {2} LF, {3} LLF, {4} Upwind <-non-conservative!
etpfix      = 0.90;     % {#} Harten's sonic entropy fix value, {0} no entropy fix

%% Space Discretization
nx  = 60;                      % number of cells
x   = linspace(0,1,nx);         % Physical domain -x
dx  = max(x(2:end)-x(1:end-1)); % delta x

%% Load Initial Condition
[z0,ux0,t0,p0,rho0,E0] = SSBGK_IC1d(x,IC_case);

%% Load Initial Cut Function
xa = 0;   % buffer left boundary
xb = 1;   % buffer right boundary
%h0  = cutfunc(x,xa,xb);  % Physical Cut Function
%h0 = ones(size(x));
h0 = zeros(size(x));

%% Discretization of the Velocity Space
% Microscopic Velocity Discretization (using Discrete Ordinate Method)
% that is to make coincide discrete values of microscopic velocities with
% values as the value points for using a quadrature method, so that we can
% integrate the velocity probability distribution to recover our
% macroscopics properties.
switch quad

    case{1} % Newton Cotes Quadrature:
    V  = [-20,20];  % range: a to b
    nv = 200;       % nodes desired (may not the actual value)
    [c,w,k] = cotes_xw(V(1),V(2),nv,5); % Using Netwon Cotes Degree 5

    case{2} % Gauss Hermite Quadrature:
    nv = 60;          % nodes desired (the actual value)
    [c,w] = GaussHermite(nv); % for integrating range: -inf to inf
    k = 1;            % quadrature constant.
    w = w.*exp(c.^2); % weighting function of the Gauss-Hermite quadrature

    otherwise
        error('Order must be between 1 and 2');
end

%% Applying DOM
% The actual nv value will be computed using 'lenght' vector function:
nv = length(c);
% Remap velocity points
    c = repmat(c,1,nx);     w = repmat(w,1,nx);
% Remap classical IC
    [rho0,ux0,p0] = apply_DOM(rho0,ux0,p0,nv);
% Remap Semiclassical IC
    [z0,t0,E0] = apply_DOM(z0,t0,E0,nv);
% Remap h coeficient
  h0 = repmat(h0,nv,1);

%% Semi0-classical Equilibrium Distribution Function
M0 = f_equilibrium_1d(z0,ux0,c,t0,theta);

%% Load initial Conditions and Spliting of information
M = M0; rho = rho0; ux = ux0; t = t0; p = p0; z = z0; h = h0; E = E0;

%% Split ICs in R and L
rhor = h.*rho0;     rhol = (1-h).*rho0;
uxr = h.*ux0;       uxl = (1-h).*ux0;
pr = h.*p0;         pl = (1-h).*p0;

[zr,~,tr,~] = macroproperties1d(rhor,rhor.*uxr,pr+0.5*rhor.*uxr.^2,nx,nv,theta);
[zl,~,tl,~] = macroproperties1d(rhol,rhol.*uxl,pl+0.5*rhol.*uxl.^2,nx,nv,theta);

fr = f_equilibrium_1d(zr,uxr,c,tr,theta);

%% Main Loop
% Compute next time step
dt = dx*CFL/max(c(:,1));
time  = 0:dt:tEnd;
dtdx = dt/dx;

Ml = f_equilibrium_1d(zl,uxl,c,tl,theta) ;
Ml(isnan(Ml)) = 0;     fr(isnan(fr)) = 0;
f = fr + Ml; % computed here for ploting purposes

count = 1; % iteration counter
tic;
for tsteps = time

    % Compute M_eq for the entire domain
    M_eq = f_equilibrium_1d(z,ux,c,t,theta);     % total % theta = 0;

    % Break total information into r and l
    if (tsteps>1)
        rhol = (1-h).*rho;
        uxl = (1-h).*ux;
        pl = (1-h).*p;
    end

    % Plot IC
    %if plot_figs == 1; surf(f); end;

    % Compute vector 'q'
    q = [rhol(1,:) ; rhol(1,:).*uxl(1,:) ; pl(1,:)+0.5*rhol(1,:).*uxl(1,:).^2];

    % Update Physical cut function 'h'
    h_next = h; % this means: fixed buffer assumption

% uncomment for BGK ******************************************************
%     % Evaluate Modified Boltzmann BGK
%     [fr_next] = ModSBBGK(h,h_next,M_eq,fr,Ml,c,flxtype);
%
%     % Compute macroscopic moments
%     [rhor,rhour,Er] = macromoments1d(k,w,fr_next,c);
%
%     % macroscopic properties
%     [zr,uxr,tr,pr] = macroproperties1d(rhor,rhour,Er,nx,nv,theta);
%
%         rhol = zeros(size(x));
%         rhoul = zeros(size(x));
%         El = zeros(size(x));
%         Ml_next = zeros(size(fr));
% uncomment for BGK ******************************************************

% uncomment for EULER*****************************************************
    rhor = zeros(size(x));
    rhour = zeros(size(x));
    Er = zeros(size(x));
    fr_next = zeros(size(Ml));

    % Evaluate Modificed Roe Euler Solver
    [rhol,rhoul,El] = ModEuler(h,h_next,q,fr_next,c);

    %plot partial result
    if plot_figs == 1;
        subplot(1,3,1); plot(x,rhol,'o'); title('Density');
        subplot(1,3,2); plot(x,rhoul./rhol,'o'); title('Velocity');
        subplot(1,3,3); plot(x,El,'o'); title('Energy');
    end

    % macroscopic properties
    [zl,uxl,tl,pl] = macroproperties1d(rhol,rhoul,El,nx,nv,theta);

    % Apply DOM
    [tl,zl,uxl] = apply_DOM(tl,zl,uxl,nv);

    % Compute Ml_next
    Ml_next = f_equilibrium_1d(zl,uxl,c,tl,theta) ;

% uncomment for EULER*****************************************************

    % New time step info: sum left and righ values with 'NaN' filter sum
    % function.
    Ml_next(isnan(Ml)) = 0;     fr_next(isnan(fr_next)) = 0;
    f   = fr_next + Ml_next;    % total f
    rho = nansum([rhol;rhor]);  % total density
    rhou= nansum([rhoul;rhour]);% total velocity in x
    E   = nansum([El;Er]);      % total persure

    %Apply Neumann BC's in total variables
    f(:,1)  = f(:,2);           f(:,end)  = f(:,end-1);
    rho(:,1)= rho(:,2);         rho(:,end)= rho(:,end-1);
    rhou(:,1) = rhou(:,2);      rhou(:,end) = rhou(:,end-1);
    E(:,1)  = E(:,2);           E(:,end)  = E(:,end-1);

    % Recover Semiclassical Conditions for next time step
    [z,ux,t,p] = macroproperties1d(rho,rhou,E,nx,nv,theta);

    %Apply DOM
    [rho,ux,p] = apply_DOM(rho,ux,p,nv);
    [t,z,E] = apply_DOM(t,z,E,nv);

    % Update information
    fr = fr_next;

    % update counter
    count = count+1;

    % update plot
    drawnow
end
toc;
% write/plot final output
%plot(x,r_total,'o');


% lapplot() -  Compute the discrete laplacian of EEG scalp distribution(s)
%
% Usage:
%   >> laplace = lapplot(map,eloc_file,draw)
%
% Inputs:
%    map        - Activity levels, size (nelectrodes,nmaps)
%    eloc_file	- Electrode location filename (.loc file)
%                 For format, see  >> topoplot example
%    draw       - If defined, draw the map(s) {default: no}
%
% Output:
%    laplace    - Laplacian map, size (nelectrodes,nmaps)
%
% Note: uses del2()
%
% Author: Scott Makeig, SCCN/INC/UCSD, La Jolla, 1998
%
% See also: topoplot(), gradplot()

% Copyright (C) Scott Makeig, SCCN/INC/UCSD, La Jolla, 1998
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

% 01-25-02 reformated help & license, added links -ad

function [laplac] = lapplot(map,filename,draw)

if nargin < 2
	help lapplot;
	return;
end;

MAXCHANS = size(map,1);
GRID_SCALE = 2*MAXCHANS+5;
MAX_RADIUS = 0.5;

% ---------------------
% Read the channel file
% ---------------------
if isstr( filename )
	fid = fopen(filename);
	locations = fscanf(fid,'%d %f %f %s',[7 MAXCHANS]);
	fclose(fid);
	locations = locations';
	Th = pi/180*locations(:,2);   % convert degrees to rads
	Rd = locations(:,3);
	ii = find(Rd <= MAX_RADIUS); % interpolate on-scalp channels only
	Th = Th(ii);
	Rd = Rd(ii);
	[x,y] = pol2cart(Th,Rd);
else
	x = real(filename);
	y = imag(filename);
end;

% ---------------------------------------------------
% Locate nearest position of an electrode in the grid
% ---------------------------------------------------
xi = linspace(-0.5,0.5,GRID_SCALE);   % x-axis description (row vector)
yi = linspace(-0.5,0.5,GRID_SCALE);   % y-axis description (row vector)
for i=1:MAXCHANS
   [useless_var horizidx(i)] = min(abs(y(i) - xi));    % find pointers to electrode
   [useless_var vertidx(i)] = min(abs(x(i) - yi));     % positions in Zi
end;

% -----------------
% Compute laplacian
% -----------------
for i=1:size(map,2)
   	[Xi,Yi,Zi] = griddata(y,x,map(:,i),yi',xi, 'v4');   % interpolate data

   	laplac2D = del2(Zi);
	positions = horizidx + (vertidx-1)*GRID_SCALE;
	laplac(:,i) = laplac2D(positions(:));

	% ------------------
	% Draw laplacian map
	% ------------------
	if exist('draw');
        mask = (sqrt(Xi.^2+Yi.^2) <= MAX_RADIUS);
        laplac2D(find(mask==0)) = NaN;

		subplot(ceil(sqrt(size(map,2))), ceil(sqrt(size(map,2))), i);
		contour(laplac2D);
		title( int2str(i) );

% %%% Draw Head %%%%
ax = axis;
width = ax(2)-ax(1);
axis([ax(1)-width/3 ax(2)+width/3 ax(3)-width/3 ax(4)+width/3])
steps = 0:2*pi/100:2*pi;
basex = .18*MAX_RADIUS;
tip = MAX_RADIUS*1.15;
base = MAX_RADIUS-.004;
EarX = [.497 .510 .518 .5299 .5419 .54 .547 .532 .510 .489];
EarY = [.0555 .0775 .0783 .0746 .0555 -.0055 -.0932 -.1313 -.1384 -.1199];

HCOLOR = 'k';
HLINEWIDTH = 1.8;

% Plot Head, Ears, Nose
hold on
plot(1+width/2+cos(steps).*MAX_RADIUS*width,...
     1+width/2+sin(steps).*MAX_RADIUS*width,...
    'color',HCOLOR,'Linestyle','-','LineWidth',HLINEWIDTH); % head

plot(1+width/2+[.18*MAX_RADIUS*width;0;-.18*MAX_RADIUS*width],...
     1+width/2+[base;tip;base]*width,...
    'Color',HCOLOR,'LineWidth',HLINEWIDTH);                 % nose

plot(1+width/2+EarX*width,...
     1+width/2+EarY*width,...
           'color',HCOLOR,'LineWidth',HLINEWIDTH)           % l ear
plot(1+width/2-EarX*width,...
     1+width/2+EarY*width,...
           'color',HCOLOR,'LineWidth',HLINEWIDTH)           % r ear

hold off
axis off

	end;
end;

return;


function [label, centroid, dis] = fkmeans(X, k, options)
% FKMEANS Fast K-means with optional weighting and careful initialization.
% [L, C, D] = FKMEANS(X, k) partitions the vectors in the n-by-p matrix X
% into k (or, rarely, fewer) clusters by applying the well known batch
% K-means algorithm. Rows of X correspond to points, columns correspond to
% variables. The output k-by-p matrix C contains the cluster centroids. The
% n-element output column vector L contains the cluster label of each
% point. The k-element output column vector D contains the residual cluster
% distortions as measured by total squared distance of cluster members from
% the centroid.
%
% FKMEANS(X, C0) where C0 is a k-by-p matrix uses the rows of C0 as the
% initial centroids instead of choosing them randomly from X.
%
% FKMEANS(X, k, options) allows optional parameter name/value pairs to
% be specified. Parameters are:
%
%   'weight' - n-by-1 weight vector used to adjust centroid and distortion
%              calculations. Weights should be positive.
%   'careful' - binary option that determines whether "careful seeding"
%               as recommended by Arthur and Vassilvitskii is used when
%               choosing initial centroids. This option should be used
%               with care because numerical experiments suggest it may
%               be counter-productive when the data is noisy.
%
% Notes
% (1) The careful seeding procedure chooses the first centroid at random
% from X, and each successive centroid from the remaining points according
% to the categorical distribution with selection probabilities proportional
% to the point's minimum squared Euclidean distance from the already chosen
% centroids. This tends to spread the points out more evenly, and, if the
% data is made of k well separated clusters, is likely to choose an initial
% centroid from each cluster. This can speed convergence and reduce the
% likelihood of getting a bad solution [1]. However, in experiments where
% 5% uniformly distributed noise data was added to such naturally clustered
% data the results were frequently worse then when centroids were chosen at
% random.
% (2) If, as is possible, a cluster is empty at the end of an iteration,
% then there may be fewer than k clusters returned. In practice this seems
% to happen very rarely.
% (3) Unlike the Mathworks KMEANS this implementation does not perform a
% final, slow, phase of incremental K-means ('onlinephase') that guarantees
% convergence to a local minimum.
%
% References
% [1] "k-means++: The Advantages of Careful Seeding", by David Arthur and
% Sergei Vassilvitskii, SODA 2007.

n = size(X,1);

% option defaults
weight = 0; % uniform unit weighting
careful = 0;% random initialization

if nargin == 3
    if isfield(options, 'weight')
        weight = options.weight;
    end
    if isfield(options,'careful')
        careful = options.careful;
    end
end

% If initial centroids not supplied, choose them
if isscalar(k)
    % centroids not specified
    if careful
        k = spreadseeds(X, k);
    else
        k = X(randsample(size(X,1),k),:);
    end
end

% generate initial labeling of points
[~,label] = max(bsxfun(@minus,k*X',0.5*sum(k.^2,2)));
k = size(k,1);

last = 0;

if ~weight
    % code defactoring for speed
    while any(label ~= last)
        % remove empty clusters
        [~,~,label] = unique(label);
        % transform label into indicator matrix
        ind = sparse(label,1:n,1,k,n,n);
        % compute centroid of each cluster
        centroid = (spdiags(1./sum(ind,2),0,k,k)*ind)*X;
        % compute distance of every point to each centroid
        distances = bsxfun(@minus,centroid*X',0.5*sum(centroid.^2,2));
        % assign points to their nearest centroid
        last = label;
        [~,label] = max(distances);
    end
    dis = ind*(sum(X.^2,2) - 2*max(distances)');
else
    while any(label ~= last)
        % remove empty clusters
        [~,~,label] = unique(label);
        % transform label into indicator matrix
        ind = sparse(label,1:n,weight,k,n,n);
        % compute centroid of each cluster
        centroid = (spdiags(1./sum(ind,2),0,k,k)*ind)*X;
        % compute distance of every point to each centroid
        distances = bsxfun(@minus,centroid*X',0.5*sum(centroid.^2,2));
        % assign points to their nearest centroid
        last = label;
        [~,label] = max(distances);
    end
    dis = ind*(sum(X.^2,2) - 2*max(distances)');
end
label = label';

% Code below this line reused from the file exchange submission K-means++
% (http://www.mathworks.com/matlabcentral/fileexchange/28901-k-means) in
% accordance with the license:
% vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv
% Copyright (c) 2010, Michael Chen
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in the
%       documentation and/or other materials provided with the distribution
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
% IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
% THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
% PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
% CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
% EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
% PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
% PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
% NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
% SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
% ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
%
function D = sqrdistance(A, B)
% Square Euclidean distances between all sample pairs
% A:  n1 x d data matrix
% B:  n2 x d data matrix
% WB: n2 x 1 weights for matrix B
% D: n2 x n1 pairwise square distance matrix
%    D(i,j) is the squared distance between A(i,:) and B(j,:)
% Written by Michael Chen (sth4nth@gmail.com). July 2009.
n1 = size(A,1); n2 = size(B,2);
m = (sum(A,1)+sum(B,1))/(n1+n2);
A = bsxfun(@minus,A,m);
B = bsxfun(@minus,B,m);
D = full((-2)*(A*B'));
D = bsxfun(@plus,D,full(sum(B.^2,2))');
D = bsxfun(@plus,D,full(sum(A.^2,2)))';
end

function [S, idx] = spreadseeds(X, k)
% X: n x d data matrix
% k: number of seeds
% reference: k-means++: the advantages of careful seeding.
% by David Arthur and Sergei Vassilvitskii
% Adapted from softseeds written by Mo Chen (mochen@ie.cuhk.edu.hk),
% March 2009.
[n,d] = size(X);
idx = zeros(k,1);
S = zeros(k,d);
D = inf(n,1);
idx(1) = ceil(n.*rand);
S(1,:) = X(idx(1),:);
for i = 2:k
    D = min(D,sqrdistance(S(i-1,:),X));
    idx(i) = find(cumsum(D)/sum(D)>rand,1);
    S(i,:) = X(idx(i),:);
end
end

end


% Automatic flight control of F8 aircraft using model predictive control
% and various models

% MPC execution time for sine3 models
% 83.5243, 188.2009, 2.1348e+03
% 79.5731, 179.1829, 2.1387e+03

clear all, close all, clc

SystemModel = 'F8';

figpath = ['../FIGURES/',SystemModel,'/'];mkdir(figpath)
datapath = ['../DATA/',SystemModel,'/'];mkdir(datapath)
addpath('../utils');

%% Load Models
Nvar = 3;
InputSignalTypeModel = 'sine3';             % Use actuation to collect training data
InputSignalTypeModel_Validation = 'sine2';  % Use actuation to collect validation data to test for generalization

% DelayDMDc
ModelTypeDMDc = 'DMDc'; % Model: DelayDMDc , DMDc
load(fullfile(datapath,['EX_',SystemModel,'_SI_',ModelTypeDMDc,'_',InputSignalTypeModel,'.mat']))
Models.DelayDMDc = Model;

% SINDYc
load(fullfile(datapath,['EX_',SystemModel,'_SI_SINDYc','_',InputSignalTypeModel,'.mat']))
Models.SINDYc = Model;

% NARX
NARX_SUBSTRACT_MEAN = 0;
load(fullfile(datapath,['EX_',SystemModel,'_SI_NARX','_',InputSignalTypeModel,'.mat']))
Models.NARX = Model;

%% Get training data
ENSEMBLE_DATA = 0;
ONLY_TRAINING_LENGTH = 1;
InputSignalType = InputSignalTypeModel;
Ndelay = Models.DelayDMDc.Ndelay;
getTrainingData

%% Get validation data: Model comparison
InputSignalType = InputSignalTypeModel_Validation;
Ndelay = Models.DelayDMDc.Ndelay;
tspanV =[tspan(end):dt:150];

% Forcing
if strcmp(InputSignalTypeModel_Validation,'sine2')
    forcing = @(x,t) [(.05* (sin(0.7*t).*sin(.1*t).*sin(.2*t).*sin(.05*t)) )];
elseif strcmp(InputSignalTypeModel_Validation,'sine3')
    forcing = @(x,t) (.5*sin(5*t).*sin(.5*t)+0.1).^3;
end

% Reference
[tA,xA] = ode45(@(t,x)F8Sys(t,x,forcing(x,t)),tspanV,x(end,1:3),options);   % true model
u_valid = forcing(0,tA)';

% DelayDMDc / DMDc
if Ndelay == 1
    x0      = [x(end,1:Nvar)];
    Hunew   = [u(end),u_valid(1:end-1)];
    [xB,tB] = lsim(Models.DelayDMDc.sys,Hunew,tspanV,[x0-[Models.DelayDMDc.xmean]]');
elseif Ndelay > 1
    x0      = [x(end-Ndelay+1,1:Nvar),x(end,1:Nvar)];
    Hunew   = [ u(end-Ndelay+1:end),u_valid(1:end-Ndelay);
        u(end),u_valid(1:end-1)];
    [xB,tB] = lsim(Models.DelayDMDc.sys,Hunew,tspanV,[x0-[Models.DelayDMDc.xmean]]');
    xB = xB(:,Nvar+1:2*Nvar); xB = xB + repmat(xmean,[size(xB,1) 1]);
end
xB = xB + repmat(Models.DelayDMDc.xmean,[length(tB) 1]);

% SINDYc
[tC,xC]=ode45(@(t,x)sparseGalerkinControl(t,x,forcing(x,t),Models.SINDYc.Xi(:,1:Nvar),Models.SINDYc.polyorder,Models.SINDYc.usesine),tspanV,x(end,1:Nvar),options);  % approximate

% NARX
Udummy = [u(end),u_valid(1:end)];
Hdummy = zeros(Nvar,size(Udummy,2));
if NARX_SUBSTRACT_MEAN == 1
    NARX_xmean = Models.NARX.xmean;
else
    NARX_xmean = zeros(size(Models.NARX.xmean'));
end

Hdummy(:,1) = x(end,1:Nvar)'-NARX_xmean';
[Us,Ui,Si] = preparets(Models.NARX.net,con2seq(Udummy),{},con2seq(Hdummy));
xD = Models.NARX.net(Us,Ui,Si); % Predict on validation data
xD = cell2mat(xD)'; xD = [x0;xD(1:end-1,:)];
xD(2:end,:) = xD(2:end,:) + repmat(NARX_xmean,[size(xD,1)-1 1]);

%% Show results
clear ph
h = figure;
subplot(3,1,1), box on, hold on
plot([tB(1) tB(1)],[-100 100],'--k')
plot(t,x(:,1),'Color',[.4 .4 .4],'LineWidth',1.5);
plot(tA,xA(:,1),'k','LineWidth',1.5);
plot(tB,xB(:,1),'r-','LineWidth',1.5);
plot(tC,xC(:,1),'g--','LineWidth',1.5);
plot(tA(1:end),xD(:,1),'-.','Color',[0.7,0.7,1],'LineWidth',1.5);
grid on
ylim([-0.4 0.3])
ylabel('x_1','FontSize',13)
set(gca,'FontSize',13)

subplot(3,1,2), box on, hold on
plot([tB(1) tB(1)],[-100 100],'--k')
plot(t,x(:,2),'Color',[.4 .4 .4],'LineWidth',1.5);
plot(tA,xA(:,2),'k','LineWidth',1.5);
plot(tB,xB(:,2),'r-','LineWidth',1.5);
plot(tC,xC(:,2),'g--','LineWidth',1.5);
plot(tA(1:end),xD(:,2),'-.','Color',[0.7,0.7,1],'LineWidth',1.5);
ylim([-4 0.1])
set(gca,'FontSize',13)
ylabel('x_2','FontSize',13)

subplot(3,1,3), box on, hold on
plot([tB(1) tB(1)],[-100 100],'--k')
ph(1) = plot(t,x(:,3),'Color',[.4 .4 .4],'LineWidth',1.5);
ph(2) = plot(tA,xA(:,3),'k','LineWidth',1.5);
ph(3) = plot(tB,xB(:,3),'r-','LineWidth',1.5);
ph(4) = plot(tC,xC(:,3),'g--','LineWidth',1.5);
ph(5) = plot(tA(1:end),xD(:,3),'-.','Color',[0.7,0.7,1],'LineWidth',1.5);
l1=legend(ph,'Training','Validation',ModelTypeDMDc,'SINDYc','NARX');
set(l1,'Location','NorthWest')
grid on
ylim([-0.9 0.5])
ylabel('x_3','FontSize',13)
set(gca,'FontSize',13)
xlabel('Time','FontSize',13)

set(h,'Units','Inches');
set(gcf,'Position',[1 1 6. 5.5])
pos = get(h,'Position');
set(h,'PaperPositionMode','Auto','PaperSize',[pos(3), pos(4)])
print(h,'-painters','-depsc2',  '-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_Comparison_Validation_',InputSignalTypeModel,'_',InputSignalType,'.eps'],'-r0');

%% Actuation signal
t_valid = tspanV;
clear ph
figure,box on, hold on,
ccolors = get(gca,'colororder');
plot([tA(1),tA(1)],[-15 260],':','Color',[0.4,0.4,0.4],'LineWidth',1.5)
plot(t,u,'-k','LineWidth',1);
plot(t_valid,u_valid,'-k','LineWidth',1);
grid off
ylim([min([u u_valid])+0.05*min([u u_valid]) max([u u_valid])+0.05*max([u u_valid])])
xlim([0 t_valid(end)])
xlabel('Time')
ylabel('Input')
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')
print('-depsc2', '-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_Comparison_Validation_',InputSignalTypeModel,'_',InputSignalType,'_Actuation.eps']);

%% True SINDYc model parameters
Xi0 = zeros(size(Models.SINDYc.Xi));
Xi0([2,4,5,6,8,10,16,18,19,25,35],1) = [-0.877,1,-0.215,0.47,-0.088,-0.019,3.846,-1,0.28,0.47,0.63];
Xi0([4],2) = 1;
Xi0([2,4,5,6,16,19,25,35],3) = [-4.208,-0.396,-20.967,-0.47,-3.564,6.265,46,61.4];

%% MPC
% Parameters MPC
options = optimoptions('fmincon','Algorithm','sqp','Display','none', ...
    'MaxIterations',100);
Duration = 6;                   % Run for 'Duration' time units
Ton = 0;                        % Time units when control is turned on
getMPCparams

x0n=[0.1 0 0]'; %xA(end,:)';    % Initial condition
Tcontrol = tA(end);             % Time offset to combine training, prediction and control phase

% Parameters Models
ModelCollection = {ModelTypeDMDc, 'SINDYc', 'NARX'}; % 51.8924, 46.0083, 1.4662e+03 execution times
Nmodels = length(ModelCollection);

clear Results
Results(1:Nmodels) = struct('x',[], 'u', [], 't', [], 'xref', [], 'J', [], 'elapsed_time', []);

Ts = Models.SINDYc.dt;

%%
for iM = 1:Nmodels
    select_model = ModelCollection{iM}; % DelayDMDc; DMDc; NARX ; SINDYc

    % Prepare variables
    Nt = (Duration/Ts)+1;
    uopt0    = 0;
    xhat     = x0n;
    uopt     = uopt0.*ones(Nu,1);
    xHistory = zeros(Nvar,Nt); xHistory(:,1) = xhat;
    uHistory = zeros(1,Nt); uHistory(1)   = uopt(1);
    tHistory = zeros(1,Nt); tHistory(1)   = Tcontrol;
    rHistory = zeros(1,Nt);

    % Parameters Model
    switch select_model
        case 'DelayDMDc'
            pest.dt     = Models.DelayDMDc.dt;
            pest.sys    = Models.DelayDMDc.sys;
            pest.xmean  = xmean';
            pest.udelay = zeros(1,N);
            pest.xdelay = [x(end-Ndelay+1,1:2)]';
            pest.Nxlim  = size(x,1);
        case 'DMDc'
            pest.dt = Models.DelayDMDc.dt;
            pest.sys = Models.DelayDMDc.sys;
            pest.xmean = Models.DelayDMDc.xmean';
            pest.Nxlim = size(x,1);
        case 'SINDYc'
            pest.ahat = Models.SINDYc.Xi(:,1:Nvar); % Replace with Xi0 to test true model
            pest.polyorder = Models.SINDYc.polyorder;
            pest.usesine = Models.SINDYc.usesine;
            pest.dt = Models.SINDYc.dt;
        case 'NARX'
            pest.net = Models.NARX.net;
            pest.Ndelay = 1;
            pest.udelay = zeros(1,Ndelay);
            pest.xdelay = zeros(2,length(pest.udelay));
            pest.xdelay(:,1:Ndelay) = [x(end-Ndelay+1:end,1:2)]';
            pest.Nxlim = size(x,1);
    end

    % Start simulation
    fprintf('Simulation started.  It might take a while...\n')
    tic
    for ct = 1:(Duration/Ts)

        % Set references over prediction horizon
        tref = (ct:ct+N-1).*Ts;
        xref = [xrefFUN(tref); zeros(1,N); zeros(1,N)];

        % NMPC with full-state feedback
        COSTFUN = @(u) ObjectiveFCN_models(u,xhat,N,Nu,xref,uHistory(:,ct),pest,diag(Q),R,Ru,select_model);
        CONSFUN = @(u) ConstraintFCN_models(u,uHistory(:,ct),xhat,N,LBo,UBo,LBdu,UBdu,pest,select_model);
        uopt = fmincon(COSTFUN,uopt,[],[],[],[],LB,UB,CONSFUN,options);

        % Run without constraints
%         uopt = fmincon(COSTFUN,uopt,[],[],[],[],[],[],[],options);

        % Integrate system
        xhat = rk4u(@F8Sys,xhat,uopt(1),Ts/10,10,[],0);
        xHistory(:,ct+1) = xhat;
        uHistory(:,ct+1) = uopt(1);
        tHistory(:,ct+1) = ct*Ts+Tcontrol;
        rHistory(:,ct+1) = xref(1,2);

    end
    tElapsed = toc
    fprintf('Simulation finished!\n')

    % Collect results
    Results(iM).eval_time = tElapsed;
    Results(iM).xref = rHistory;
    Results(iM).x = xHistory;
    Results(iM).u = uHistory;
    Results(iM).t = tHistory;
    Results(iM).J = evalObjectiveFCN(Results(iM).u,Results(iM).x,Results(iM).xref,diag(Q),R,Ru);
    Results(iM).elapsed_time = tElapsed;

    VIZ_SI_Validation_MPC

    if iM==Nmodels % Plot ensemble training for NN / NARX
       VIZ_SI_Validation_MPC_ensemble(ct,t_valid,u_valid,xHistory,tHistory,uHistory,Results,t,tA,xA,xB,xC,xD,select_model,SystemModel,figpath,N,InputSignalTypeModel,Nmodels,ModelCollection)
    end

end

% MPC Execution times using N=10: 120.9128, 96.8967, 2.1755e+03

%% Unforced
[tU,xU] = ode45(@(t,x)F8Sys(t,x,0,[]),tHistory,xA(end,1:3),options);   % true model

%% Show cost
clear ph
figure, hold on, box on
ph(1) = plot(Results(1).t,cumsum(Results(1).J),'-r','LineWidth',2);
ph(3) = plot(Results(1).t,cumsum(Results(3).J),'-.','Color',[0.7,0.7,1],'LineWidth',2);
ph(2) = plot(Results(1).t,cumsum(Results(2).J),'--g','LineWidth',2);

xlim([Results(1).t(1) Results(1).t(end)])
ylabel('Cost','FontSize',14)
xlabel('Time','FontSize',14)
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')

print('-depsc2', '-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_Comparison_Validation_',InputSignalTypeModel,'_',InputSignalType,'_Cost','_N_',num2str(N),'.eps']);

%% Show control stage separate
clear ph
figure, hold on, box on
ph(4) = plot(Results(1).t(2:end),(Results(1).xref(2:end)),'-k','LineWidth',2);
ph(1) = plot(Results(1).t,(Results(1).x(1,:)),'-r','LineWidth',2);
ph(3) = plot(Results(1).t,(Results(3).x(1,:)),'-.','Color',[0.7,0.7,1],'LineWidth',2);
ph(2) = plot(Results(1).t,(Results(2).x(1,:)),'--g','LineWidth',2);

l1=legend(ph,ModelCollection,'Ref');
set(l1,'Location','NorthEast')
xlim([Results(1).t(1) Results(1).t(end)])
ylim([-0.25 .25])
ylabel('AoA','FontSize',14)
xlabel('Time','FontSize',14)
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')

print('-depsc2', '-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_Comparison_',InputSignalTypeModel,'_',InputSignalType,'_AoA','_N_',num2str(N),'.eps']);

clear ph
figure, hold on, box on
ph(1) = plot(Results(1).t,(Results(1).u(:)),'-r','LineWidth',2);
ph(3) = plot(Results(1).t,(Results(3).u(:)),'-.','Color',[0.7,0.7,1],'LineWidth',2);
ph(2) = plot(Results(1).t,(Results(2).u(:)),'--g','LineWidth',2);

xlim([Results(1).t(1) Results(1).t(end)])
ylabel('Input','FontSize',14)
xlabel('Time','FontSize',14)
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')

print('-depsc2', '-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_Comparison_',InputSignalTypeModel,'_',InputSignalType,'_Input','_N_',num2str(N),'.eps']);

%% Save results
save(fullfile(datapath,['MPC_',SystemModel,'_Results_AllModels_',InputSignalTypeModel,'_',InputSignalType,'_N_',num2str(N),'.mat']),'Results')
save(fullfile(datapath,['MPC_',SystemModel,'_Results_AllModels_',InputSignalTypeModel,'_',InputSignalType,'_All','_N_',num2str(N),'.mat']))


function Ne = som_neighborhood(Ne1,n)

%SOM_NEIGHBORHOOD Calculate neighborhood matrix.
%
% Ne = som_neighborhood(Ne1,n)
%
%  Ne = som_neighborhood(Ne1);
%  Ne = som_neighborhood(som_unit_neighs(topol),2);
%
%  Input and output arguments ([]'s are optional):
%   Ne1       (matrix, size [munits m]) a sparse matrix indicating
%                      the units in 1-neighborhood for each map unit
%   [n]       (scalar) maximum neighborhood which is calculated, default=Inf
%
%   Ne        (matrix, size [munits munits]) neighborhood matrix,
%                      each row (and column) contains neighborhood
%                      values from the specific map unit to all other
%                      map units, or Inf if the value is unknown.
%
% For more help, try 'type som_neighborhood' or check out online documentation.
% See also SOM_UNIT_NEIGHS, SOM_UNIT_DISTS, SOM_UNIT_COORDS, SOM_CONNECTION.

%%%%%%%%%%%%% DETAILED DESCRIPTION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% som_neighborhood
%
% PURPOSE
%
% Calculate to which neighborhood each map unit belongs to relative to
% each other map unit, given the units in 1-neighborhood of each unit.
%
% SYNTAX
%
%  Ne = som_neighborhood(Ne1);
%  Ne = som_neighborhood(Ne1,n);
%
% DESCRIPTION
%
% For each map unit, finds the minimum neighborhood to which it belongs
% to relative to each other map unit. Or, equivalently, for each map
% unit, finds which units form its k-neighborhood, where k goes from
% 0 to n.
%
% The neighborhood is calculated iteratively using the reflexivity of
% neighborhood.
%     let  N1i  be the 1-neighborhood set a unit i
% and let  N11i be the set of units in the 1-neighborhood of any unit j in N1i
%     then N2i  (the 2-neighborhood set of unit i) is N11i \ N1i
%
% Consider, for example, the case of a 5x5 map. The neighborhood in case of
% 'rect' and 'hexa' lattices (and 'sheet' shape) for the unit at the
% center of the map are depicted below:
%
%   'rect' lattice           'hexa' lattice
%   --------------           --------------
%   4  3  2  3  4            3  2  2  2  3
%   3  2  1  2  3             2  1  1  2  3
%   2  1  0  1  2            2  1  0  1  2
%   3  2  1  2  3             2  1  1  2  3
%   4  3  2  3  4            3  2  2  2  3
%
% Because the iterative procedure is rather slow, the neighborhoods
% are calculated upto given maximal value. The uncalculated values
% in the returned matrix are Inf:s.
%
% REQUIRED INPUT ARGUMENTS
%
%  Ne1   (matrix) Each row contains 1, if the corresponding unit is adjacent
%                 for that map unit, 0 otherwise. This can be calculated
%                 using SOM_UNIT_NEIGHS. The matrix can be sparse.
%                 Size munits x munits.
%
% OPTIONAL INPUT ARGUMENTS
%
%  n     (scalar) Maximal neighborhood value which is calculated,
%                 Inf by default (all neighborhoods).
%
% OUTPUT ARGUMENTS
%
%  Ne    (matrix) neighborhood values for each map unit, size is
%                 [munits, munits]. The matrix contains the minimum
%                 neighborhood of unit i, to which unit j belongs,
%                 or Inf, if the neighborhood was bigger than n.
%
% EXAMPLES
%
%  Ne = som_neighborhood(Ne1,1);    % upto 1-neighborhood
%  Ne = som_neighborhood(Ne1,Inf);  % all neighborhoods
%  Ne = som_neighborhood(som_unit_neighs(topol),4);
%
% SEE ALSO
%
%  som_unit_neighs   Calculate units in 1-neighborhood for each map unit.
%  som_unit_coords   Calculate grid coordinates.
%  som_unit_dists    Calculate interunit distances.
%  som_connection    Connection matrix.

% Copyright (c) 1999-2000 by the SOM toolbox programming team.
% http://www.cis.hut.fi/projects/somtoolbox/

% Version 1.0beta juuso 141097
% Version 2.0beta juuso 101199

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Check arguments

error(nargchk(1, 2, nargin));

if nargin<2, n=Inf; end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Action

% initialize
if issparse(Ne1), Ne = full(Ne1); else Ne = Ne1; end
clear Ne1
[munits dummy] = size(Ne);
Ne(find(Ne==0)) = NaN;
for i=1:munits, Ne(i,i)=0; end

% Calculate neighborhood distance for each unit using reflexsivity
% of neighborhood:
%   let  N1i be the 1-neighborhood set a unit i
%   then N2i is the union of all map units, belonging to the
%        1-neighborhood of any unit j in N1i, not already in N1i
k=1;
if n>1,
  fprintf(1,'Calculating neighborhood: 1 ');
  N1 = Ne;
  N1(find(N1~=1)) = 0;
end
while k<n && any(isnan(Ne(:))),
  k=k+1;
  fprintf(1,'%d ',k);
  for i=1:munits,
    candidates = isnan(Ne(i,:));              % units not in any neighborhood yet
    if any(candidates),
      prevneigh = find(Ne(i,:)==k-1);         % neighborhood (k-1)
      N1_of_prevneigh = any(N1(prevneigh,:)); % union of their N1:s
      Nn = find(N1_of_prevneigh & candidates);
      if length(Nn), Ne(i,Nn) = k; Ne(Nn,i) = k; end
    end
  end
end
if n>1, fprintf(1,'\n'); end

% finally replace all uncalculated distance values with Inf
Ne(find(isnan(Ne))) = Inf;

return;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% faster version?

l = size(Ne1,1); Ne1([0:l-1]*(l+1)+1) = 1; Ne = full(Ne1); M0 = Ne1; k = 2;
while any(Ne(:)==0), M1=(M0*Ne1>0); Ne(find(M1-M0))=k; M0=M1; k=k+1; end
Ne([0:l-1]*(l+1)+1) = 0;


function out=errorplot(x,y,dy,plottypes,dx);
%
%  2 dimensional plot with error bars
%
%	By: P.S.Basran		6/17/03
%
%       Toronto-Sunnybrook Cancer Centre
%       Dept. Medical Physics
%       Parminder.Basran@sw.ca
%
%
%
%	Function:
%		This function produces a two dimensional plot with error bars.
%
%	Syntax:
% 		out=errorplot(x,y,dy,plottypes,dx)
%
%	Inputs:
%		    x - is a [1 x n] dimensional vector
%           y - is a [m x n] dimensional matrix for multidimensional plots
%          dy - is a [m x n] dimensional matrix whose dimensions must match those of y
%   plottypes - is a [1 x 4] dimensional vector whose entries specify the plottype in the
%               same fashion as the variable 's' in the function plot(x,y,s).
%               NOTE: current version of this requires the plottype to be specified in
%               all instances, and to be 4 characters in length.
%          dx - is a real number that specifices the dimensions of the horizontal tics
%               for each error bar. The default value is 0.25 the dimension of x.
%
%	Outputs:
%		   - plot with legend, labeled '1', '2', etc.
%
%   Example:
%
%           %For a single plot:
%
%           x=[-10:1:10];
%           y1=x.^2;
%           dy1=0.3*y1.*rand(size(x)); %
%           plottype1=[':   '];
%           errorplot(x,y1,dy1,plottype1);
%
%           % For multiple plots:
%
%           y2=0.5*x.^2;
%           dy2=0.2*y2.*rand(size(x)); %
%           plottype2=['o-- '];
%           y=[y1; y2];
%           dy=[dy1; dy2];
%           errorplot(x,y,dy,[plottype1 plottype2],1);
%

%   Modification Log:
%       June 17 - 2003: alpha version
%       March 11 - 2004: fix for the plottypes ....thanks to Sean Verret
%

error(nargchk(2,5,nargin));

if nargin <5, dx=(x(2)-x(1))/4;
    if nargin < 4, plottypes='';
        if nargin < 3, dy='';
            [a,b]=size(y);
            for i=1:a, plot(x,y(i,:)); end % default to a regular plot for 2 inputs
        end, end, end

%Provide plot and legend
[a,b]=size(y);
if a>b,
    y=y';
    [a,b]=size(y);
end

tx=[];
for i=1:a,
    plot(x,y(i,:),plottypes(4*(i-1)+1:4*i));;
    tx=[tx; 'plot ' num2str(i)];
    hold on;
end
legend(tx);

%Now the error bars
for i=1:a,
    for j=1:b,
        %horizontal lines
        line([x(j) x(j)], [y(i,j)-dy(i,j) y(i,j)+dy(i,j)]);
        %top vertical line
        line([x(j)-dx x(j)+dx], [y(i,j)+dy(i,j) y(i,j)+dy(i,j)]);
        %bottom vertical line
        line([x(j)-dx x(j)+dx], [y(i,j)-dy(i,j) y(i,j)-dy(i,j)]);
     end
end

hold off


function x=v_usasi(n,fs)
%V_USASI generates N samples of USASI noise at sample frequency FS X=(N,FS)

% This routine is based on the USASI noise defined in [1] which was later
% reissued as [2]. USASI noise is intended to simulate the long-term average
% of typical audio program material. The routine does not currently implement
% the pulsation at 2.5Hz 12.5% duty cycle that is recommended by the standard.
% Also it should probably be scaled to a well-defined power.
%
%  [1] NRSC AM Reemphasis, Deemphasize, and Broadcast Audio Transmission Bandwidth Specifications,
%      EIA-549 Standard, Electronics Industries Association , July 1988.
%  [2] NRSC AM Reemphasis, Deemphasize, and Broadcast Audio Transmission Bandwidth Specifications,
%      NRSC-1-A Standard, Sept 2007, Online: http://www.nrscstandards.org/SG/NRSC-1-A.pdf

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: v_usasi.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<2 fs=8000; end
b=[1 0 -1];
a=poly(exp(-[100 320]*2*pi/fs));

x=v_randfilt(b,a,n);


function r = mpower(a,b)
%MPOWER       Implements  a ^ b  for intervals
%
% either a and b are (interval) scalar or, b is scalar integer
%

% written  10/16/98     S.M. Rump
% modified 12/30/98     S.M. Rump  improved performance and even exponent
% modified 05/20/02     S.M. Rump  interval integer exponent checked
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    NaN corrected
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
%

  % for both a and b scalars, use .^ with improved diameter for even exponent
  if ( prod(size(a))==1 ) & ( prod(size(b))==1 )
    r = a .^ b ;
    return
  end

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  % Check integer exponent
  if isa(b,'intval') & all(inf(b)==sup(b))
    b = b.inf;
    if ~isreal(b)
      error('invalid call of intval mpower ^')
    end
    a = intval(a);
  end

  if isa(b,'double') & isreal(b) & prod(size(b))==1 & b==round(b)
    [m n] = size(a);
    if m~=n
      error('intval mpower of non-square matrix')
    end
    if b==0
      if issparse(a)
        r = intval(speye(size(a)));
      else
        r = intval(eye(size(a)));
      end
      index = isnan(a) & ( b==0 );
      if any(index(:))
        %VVVV  r(index) = NaN;
        s.type = '()'; s.subs = {index}; r = subsasgn(r,s,NaN);
        %AAAA  Matlab bug fix
      end
    else                        % b is integer
      b_is_negative = b<0;
      b = abs(b) - 1;           % abs(b) is at least 1
      r = a;
      while b>0
        if mod(b,2)==1
          r = r*a;
        end
        b = floor(b/2);
        if b~=0
          a = a*a;
        end
      end
      if b_is_negative
        r = inv(r);
      end
    end
  else
    error('invalid call of intval mpower ^')
  end

  if rndold
    setround(rndold)
  end


function [ n_data, a, b, x, fx ] = von_mises_cdf_values ( n_data )

%*****************************************************************************80
%
%% VON_MISES_CDF_VALUES returns some values of the von Mises CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 December 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Kanti Mardia and Peter Jupp,
%    Directional Statistics,
%    Wiley, 2000, QA276.M335
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real A, B, the parameters of the function.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 23;

  a_vec = [ ...
     0.0E+00, ...
     0.0E+00, ...
     0.0E+00, ...
     0.0E+00, ...
     0.0E+00, ...
     0.1E+01, ...
     0.1E+01, ...
     0.1E+01, ...
     0.1E+01, ...
     0.1E+01, ...
     0.1E+01, ...
    -0.2E+01, ...
    -0.1E+01, ...
     0.0E+01, ...
     0.1E+01, ...
     0.2E+01, ...
     0.3E+01, ...
     0.0E+00, ...
     0.0E+00, ...
     0.0E+00, ...
     0.0E+00, ...
     0.0E+00, ...
     0.0E+00 ];

  b_vec = [ ...
     0.1E+01, ...
     0.1E+01, ...
     0.1E+01, ...
     0.1E+01, ...
     0.1E+01, ...
     0.2E+01, ...
     0.2E+01, ...
     0.2E+01, ...
     0.2E+01, ...
     0.2E+01, ...
     0.2E+01, ...
     0.3E+01, ...
     0.3E+01, ...
     0.3E+01, ...
     0.3E+01, ...
     0.3E+01, ...
     0.3E+01, ...
     0.0E+00, ...
     0.1E+01, ...
     0.2E+01, ...
     0.3E+01, ...
     0.4E+01, ...
     0.5E+01 ];

  fx_vec = [ ......
    0.2535089956281180E-01, ...
    0.1097539041177346E+00, ...
    0.5000000000000000E+00, ...
    0.8043381312498558E+00, ...
    0.9417460124555197E+00, ...
    0.5000000000000000E+00, ...
    0.6018204118446155E+00, ...
    0.6959356933122230E+00, ...
    0.7765935901304593E+00, ...
    0.8410725934916615E+00, ...
    0.8895777369550366E+00, ...
    0.9960322705517925E+00, ...
    0.9404336090170247E+00, ...
    0.5000000000000000E+00, ...
    0.5956639098297530E-01, ...
    0.3967729448207649E-02, ...
    0.2321953958111930E-03, ...
    0.6250000000000000E+00, ...
    0.7438406999109122E+00, ...
    0.8369224904294019E+00, ...
    0.8941711407897124E+00, ...
    0.9291058600568743E+00, ...
    0.9514289900655436E+00 ];

  x_vec = [ ......
    -0.2617993977991494E+01, ...
    -0.1570796326794897E+01, ...
     0.0000000000000000E+00, ...
     0.1047197551196598E+01, ...
     0.2094395102393195E+01, ...
     0.1000000000000000E+01, ...
     0.1200000000000000E+01, ...
     0.1400000000000000E+01, ...
     0.1600000000000000E+01, ...
     0.1800000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.0000000000000000E+00, ...
     0.0000000000000000E+00, ...
     0.0000000000000000E+00, ...
     0.0000000000000000E+00, ...
     0.0000000000000000E+00, ...
     0.0000000000000000E+00, ...
     0.7853981633974483E+00, ...
     0.7853981633974483E+00, ...
     0.7853981633974483E+00, ...
     0.7853981633974483E+00, ...
     0.7853981633974483E+00, ...
     0.7853981633974483E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    a = 0.0;
    b = 0.0;
    x = 0.0;
    fx = 0.0;
  else
    a = a_vec(n_data);
    b = b_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function [startingvals,nu,lambda,LLs,outputParameters]=igarch_starting_values(startingvals,epsilon,fepsilon,p,q,T,errorType,igarchType,constant)
% Perform a grid search to find decent starting values for IGARCH(P,Q)
% esimtation.  If starting values are user supplied (and thus nonempty), reformats
% starting values depending on ERRORTYPE.
%
% USAGE:
%   [STARTINGVALS,NU,LAMBDA,LLS,OUTPUTPARAMETERS] = ...
%        igarch_starting_values(STARTINGVALS,EPSILON,EPSILON2,P,Q,T,ERRORTYPE,IGARCHTYPE,CONSTANT)
%
% INPUTS:
%   STARTINGVALS     - A vector of starting values or empty to perform a grid search
%   EPSILON          - A column of mean zero data
%   FEPSILON         - Either abs(EPSILON) or EPSILON.^2, depending on IGARCHTYPE
%   P                - Positive, scalar integer representing the number of
%                      symmetric innovations
%   Q                - Non-negative, scalar integer representing the number
%                      of lags of conditional variance (0 for ARCH)
%   T                - Length of EPSILON
%   ERRORTYPE        - [OPTIONAL] The error distribution used, valid types are:
%                       'NORMAL'    - Gaussian Innovations [DEFAULT]
%                       'STUDENTST' - T distributed errors
%                       'GED'       - Generalized Error Distribution
%                       'SKEWT'     - Skewed T distribution
%   IGARCHTYPE       - [OPTIONAL] The type of variance process, either
%                        1 - Model evolves in absolute values
%                        2 - Model evolves in squares [DEFAULT]
%   CONSTANT         - [OPTIONAL] Logical value indicating whether model
%                       should include a constant.  Default is true (include).
%
% OUTPUTS:
%   STARTINGVALS     - A vector of starting values (CONSTANT+p+q) by 1
%   NU               - Distribution kurtosis parameter, empty if not applicable
%   LAMBDA           - Distribution asymmetry parameter, empty if not applicable
%   LLS              - A vector of log likelihoods corresponding to OUTPUTPARAMETERS
%   OUTPUTPARAMETERS - A matrix of alternative starting values, sorted by log likelihood
%
% COMMENTS:
%   See also IGARCH

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 7/12/2009

%Initialize variables
LLs=[];
outputParameters=[];

%No starting values provided
if isempty(startingvals)
    nu=[];
    lambda=[];

    %Procedure is to find best starting values, using a grid search find values for normal, then

    %Possible starting values based on commonly estimated values
    a=[.05 .1 .2];
    la=length(a);
    ab=1;
    lb=length(ab);

    %Many outputParameters and LLs
    outputParameters=zeros(la*lb,1+p+q-1);
    LLs=zeros(la,1);

    %Adjustment is needed to the intercept.  Assumes normality
    if igarchType==1
        adjFactor=sqrt(2/pi);
        backCast=mean(abs(epsilon));
    else
        adjFactor=1;
        backCast=cov(epsilon);
    end

    %Use an index to count
    index=1;

    for i=1:la
        %Loop over a
        alpha=a(i);
        tempAlpha=alpha*ones(p,1)/p;
        %Beta must satisfy the unit root
        beta=1-sum(tempAlpha);
        %Pick omega to match the unconditional
        if constant
            omega=backCast*.01*adjFactor;
        else
            omega = [];
        end
        %Build the parameter vector

        if q==0
            parameters=[omega; tempAlpha];
        else
            parameters=[omega; tempAlpha; beta*ones(q-1,1)/q];
        end
        %Set the output parameters
        outputParameters(index,:)=parameters';
        %Set the log likelihoods
        LLs(index)=igarch_likelihood(parameters, epsilon, fepsilon, p, q, 1, igarchType, constant, backCast, T, false);
        %Increment
        index=index+1;
    end
    %Sort the LLs so the best (lowest, since we minimize the -1*LL)
    [LLs,index]=sort(LLs);
    %Order the starting values
    startingvals=outputParameters(index(1),:)';
    %Order the ouputParameters
    outputParameters=outputParameters(index,:);
    %Use generic values for nu and lambda if needed
    if errorType==2
        nu=8;
    elseif errorType==3
        nu=1.9;
    elseif errorType==4
        nu=8;
        lambda=-.1;
    end
else
    %Values provided, only parse them
    nu=[];
    lambda=[];
    if errorType==2 || errorType==3 || errorType==4
        nu=startingvals(constant+p+q);
    end
    if errorType==4
        lambda=startingvals(constant+p+q+1);
    end
    startingvals=startingvals(1:constant+p+q-1);
end


function [ relpos, psize3, delta_midy ] = gen2dOffset_v2( box1, box2, issplitting )
%GEN2DOFFSET_V2 compute the rotation and offsets between two sibling boxes
% INPUT: box1, box2 - 3d box of base box with absolute position
%
% OUTPUT: relpos - 13d vector (rp of box2 relative to box1), including:
%                    (1) relative orientation - 1 bit degree
%                    (3) offsets - 2 bits
%                    (4) 16 classes (4 classes along each axis) - 8 bits
%                    (5) attachments (4 possible cases) - 4 bits
%                    (6) alignments (aligned along each axis) - 2 bit

% box1 info
pcent = box1(1:3);
pfront = box1(4:6);
pup = box1(7:9);
psize = box1(10:12);
paxes = cross(pfront,pup);
paxes = paxes/norm(paxes,2);

% box2 info
ccent = box2(1:3);
cfront = box2(4:6);
cup = box2(7:9);
csize = box2(10:12);
caxes = cross(cfront,cup);
caxes = caxes/norm(caxes,2);

transform = genTransMat(pfront,pup);
ncent = ccent-pcent;
ncent = transform*ncent; % child's center position in parent's local frame
nfront = cfront;
nfront = transform*nfront; % child's front direction in parent's local frame
nfront = nfront/norm(nfront,2);
nup = cup;
nup = transform*nup; % child's up direction in parent's local frame
nup = nup/norm(nup,2);
naxes = cross(nfront,nup);
naxes = naxes/norm(naxes,2);

% 4 corner points of parent box under its own local frame
plfront = [1;0;0];
plaxes = [0;0;1];
ppoints(:,1) = psize(1)*plfront/2 + psize(3)*plaxes/2;
ppoints(:,2) = psize(1)*plfront/2 - psize(3)*plaxes/2;
ppoints(:,3) = -psize(1)*plfront/2 - psize(3)*plaxes/2;
ppoints(:,4) = -psize(1)*plfront/2 + psize(3)*plaxes/2;

% 4 corner points of child box under its parent's local frame
cpoints(:,1) = ncent + csize(1)*nfront/2 + csize(3)*naxes/2;
cpoints(:,2) = ncent + csize(1)*nfront/2 - csize(3)*naxes/2;
cpoints(:,3) = ncent - csize(1)*nfront/2 - csize(3)*naxes/2;
cpoints(:,4) = ncent - csize(1)*nfront/2 + csize(3)*naxes/2;

% the relative positions
pmaxx = max(ppoints(1,:));
pmaxy = max(ppoints(3,:));
pminx = min(ppoints(1,:));
pminy = min(ppoints(3,:));
cmaxx = max(cpoints(1,:));
cmaxy = max(cpoints(3,:));
cminx = min(cpoints(1,:));
cminy = min(cpoints(3,:));

delta_midy = 0;
if(issplitting==1)
    pmidy = (pmaxy+pminy)/2;
    cmidy = (cmaxy+cminy)/2;
    delta_midy = pmidy-cmidy;
    pmaxy = cmaxy;
    pminy = cminy;
end
psize3 = pmaxy-pminy;

% relative position class along x axis
delta_x = [pminx-cmaxx;pminx-cminx;pmaxx-cmaxx;pmaxx-cminx];
[~,classxindex] = min(abs(delta_x));
offsets(1) = delta_x(classxindex);

% relative position class along y axis
delta_y = [pminy-cmaxy;pminy-cminy;pmaxy-cmaxy;pmaxy-cminy];
[~,classyindex] = min(abs(delta_y));
offsets(2) = delta_y(classyindex);

% alignment info
attachindex = 1;
attacheps = 0.011; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if(abs(offsets(1))<attacheps)
    attachindex = attachindex + 1;
end
if(abs(offsets(2))<attacheps)
    attachindex = attachindex + 2;
end
attachvec = zeros(4,1);
attachvec(attachindex) = 1;

% form the relpos vector
rot_eps = 0.01;
alignment = [1;0;0;0;0];
if(abs(nfront(1)-1)<rot_eps)
    alignment = [0;1;0;0;0]; % nfront = [1,0,0];
end
if(abs(nfront(1)+1)<rot_eps)
    alignment = [0;0;1;0;0]; % nfront = [-1,0,0];
end
if(abs(nfront(3)-1)<rot_eps)
    alignment = [0;0;0;1;0]; % nfront = [0,0,1];
end
if(abs(nfront(3)+1)<rot_eps)
    alignment = [0;0;0;0;1]; % nfront = [0,0,-1];
end

if(nfront(1)==0)
    if(nfront(3)>0)
        degree = -0.5;
    else
        degree = 0.5;
    end
else
    degree = atand(nfront(3)/nfront(1));
    if(nfront(1)<0)
        if(degree<0)
            degree = degree-180;
        else
            degree = degree+180;
        end
    end
    degree = mod(degree,360)/180-1;
end
if(abs(degree)>1)
        o=0;
    end

classindex = classxindex + (classyindex-1)*4;
classvec = zeros(16,1);
classvec(classindex) = 1;
relpos = [degree;offsets(1);offsets(2);classvec;attachvec;alignment];

% tmpcent = pcent;
% tmpfsize = (max(cmaxx,pmaxx)-min(cminx,pminx));
% tmpasize = (max(cmaxy,pmaxy)-min(cminy,pminy));
% tmpobb = [tmpcent;plfront;0;1;0;tmpfsize;1;tmpasize];
% draw3dOBB_v2(box1,'r');
% draw3dOBB_v2(box2,'g');
% draw3dOBB_v2(tmpobb,'y');

end