%% batman
% Below is a demonstration of the features of the |batman| function

%% Syntax
% |[x,y]=batman(n);|
% |[V]=batman(n);|

%% Description
% The |batman| function implements a particular version of the so called
% batmat-equation, a curve defining the batman logo. The input for this
% function is the number of desired points n. The user may request a sigle
% nx2 output array or two nx1 arrays (x and y coordinates).
%
% This is a MATLAB implementation of the parameterised Batman equation
% presented by Jerome White (http://www.talljerome.com/ @talljerome,
% https://youtu.be/oaIsCJw0QG8), in particular the form presented here:
% https://www.desmos.com/calculator/ajnzwedvql
% Modification: The batman is scaled to be 2 in width.

%% Examples

clear; close all; clc;

%% Example 1:

n=250; %Number of points on curve
[x,y]=batman(n);

%%

cFigure; hold on;
xlabel('x'); ylabel('y');
plot([x;x(1)],[y;y(1)],'k.-','LineWidth',3,'MarkerSize',25);
axis tight; axis equal;
set(gca,'FontSize',25);
grid on; box on;
gdrawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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 3 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, see <http://www.gnu.org/licenses/>.


function [err,time,solver,eqn] = femStokes(node,elem,pde,bdFlag,option,varargin)
%% FEMSTOKES solve the Stokes equation by various finite element methods
%
%   FEMSTOKES computes approximations to the Stokes equation on a
%   sequence of meshes obtained by uniform refinement of a input mesh.
%
% Created by Ming Wang at Nov., 2012.
%
% See also femPoisson femrateStokes
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

%% Default setting of mesh and pde data
if ~exist('node','var') || ~exist('elem','var')
    [node,elem] = squaremesh([0,1,0,1],0.125);  % default mesh is a square
end
if ~exist('option','var'), option = []; end
if ~exist('pde','var')
    pde = Stokesdata1;                          % default data
end
if ~exist('bdFlag','var')
    bdFlag = setboundary(node,elem,'Dirichlet');
end

%% Parameters
option = femoption(option);
maxIt = option.maxIt;   maxN = option.maxN; L0 = option.L0;
option = femStokesoption(option);
elemType = option.elemType; refType = option.refType;

%% Generate an initial mesh
for k = 1:L0
    if strcmp(option.refType,'red')
        [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
    elseif strcmp(option.refType,'bisect')
        [node,elem,bdFlag] = uniformbisect(node,elem,bdFlag);
    end
end

%% Initialize err
erruH1 = zeros(maxIt,1); errpL2 = zeros(maxIt,1);
errTime = zeros(maxIt,1); solverTime = zeros(maxIt,1);
assembleTime = zeros(maxIt,1); meshTime = zeros(maxIt,1);
itStep = zeros(maxIt,1);  stopErr = zeros(maxIt,1); flag = zeros(maxIt,1);
N = zeros(maxIt,1);

%% Finite Element Method
for k = 1:maxIt
    % solve the equation
    switch upper(elemType)
        case 'P2P1'
            [u,p,edge,A,eqn,info] = StokesP2P1(node,elem,pde,bdFlag,option);
        case 'P2P0'
            [u,p,edge,A,eqn,info] = StokesP2P0(node,elem,pde,bdFlag,option);
        case 'ISOP2P1'
            [u,p,edge,A,eqn,info] = StokesisoP2P1(node,elem,pde,bdFlag,option);
        case 'ISOP2P0'
            [u,p,edge,A,eqn,info] = StokesisoP2P0(node,elem,pde,bdFlag,option);
        case 'CRP0'
            [u,p,edge,A,eqn,info] = StokesCRP0(node,elem,pde,bdFlag,option);
        case 'CRP1'
            [u,p,edge,A,eqn,info] = StokesCRP1(node,elem,pde,bdFlag,option);
        case 'MINI'
            [u,p,edge,A,eqn,info] = StokesMini(node,elem,pde,bdFlag,option);
        case 'P1BP1'
            [u,p,edge,A,eqn,info] = StokesP1bP1(node,elem,pde,bdFlag,option);
    end
    % compute error
    tic;
%     if strcmp(elemType,'P1BP1')
%         uI = Lagrangeinterpolate(pde.exactu,node,elem);
%     elseif strcmp(elemType(1:2),'CR')
%         uI = Lagrangeinterpolate(pde.exactu,node,elem,'CR',edge);
%     end
    if strcmp(elemType,'P2P0') || strcmp(elemType,'P2P1') || ...
       strcmp(elemType,'isoP2P0') || strcmp(elemType,'isoP2P1')
        uI = pde.exactu([node; (node(edge(:,1),:)+node(edge(:,2),:))/2]);
    elseif strcmp(elemType,'CRP0') || strcmp(elemType,'CRP1')
        uI = pde.exactu((node(edge(:,1),:)+node(edge(:,2),:))/2);
    elseif strcmp(elemType,'Mini')
        uI = pde.exactu(node);
    elseif strcmp(elemType,'P1bP1')
        % bubble part won't be taken in the error computation
        Nv = size(node,1); NT = size(elem,1);
        u0 = pde.exactu(node);
        uI = u;
        uI([(1:Nv)'; NT+Nv+(1:Nv)']) = u0(:);
    end
    erruH1(k) = sqrt((u-uI(:))'*A*(u-uI(:)));
    errpL2(k) = getL2error(node,elem,pde.exactp,p);
    errTime(k) = toc;
    % record time
    solverTime(k) = info.solverTime;
    assembleTime(k) = info.assembleTime;
    if option.printlevel>1
        fprintf('Time to compute the error %4.2g s \n H1 err %4.2g    L2err %4.2g \n',...
            errTime(k),erruH1(k), errpL2(k));
    end
    % record solver information
    itStep(k) = info.itStep;
%    stopErr(k) = info.stopErr;
%    flag(k) = info.flag;
    % plot
    N(k) = size(node,1);
    if option.plotflag && N(k) < 2e3 % show mesh and solution for small size
        if length(p) == size(elem,1) % piecewise constant function
            p = recoverP02P1(node,elem,p);
        end
        figure(1);  showresult(node,elem,p);
    end
    if N(k) > maxN
        break;
    end
    % refine mesh
    tic;
    if strcmp(refType,'red')
        [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
    elseif strcmp(refType,'bisect')
        [node,elem,bdFlag] = uniformbisect(node,elem,bdFlag);
    end
    meshTime(k) = toc;
end
h = 1./sqrt(N(1:k));

%% Plot convergence rates
if option.rateflag
    figure;
    set(gcf,'Units','normal');
    set(gcf,'Position',[0.25,0.25,0.55,0.4]);
    subplot(1,2,1)
    showrateh(h,erruH1(1:k),1,'k-+','|u_I-u_h|_1');
    subplot(1,2,2)
    showrateh(h,errpL2(1:k),1,'m-+','||p-p_h||');
end

%% Output
err = struct('N',N,'uH1',erruH1(1:k),'pL2',errpL2(1:k));
time = struct('N',N,'err',errTime(1:k),'solver',solverTime(1:k), ...
              'assmble',assembleTime(1:k),'mesh',meshTime(1:k));
solver = struct('N',N(1:k),'itStep',itStep(1:k),'time',solverTime(1:k),...
                'stopErr',stopErr(1:k),'flag',flag(1:k));

%% Display error
ts = zeros(k,6); ts = char(ts);
display('#nodes     |u_I-u_h|_1      ||p-p_h||  ');
% format shorte
display([num2str(err.N) ts num2str(err.uH1,'%0.5e') ts num2str(err.pL2,'%0.5e')]);


function [inH2O] = kPa2inH2O(kPa)
% Convert pressure from kilopascals to inches of water.
% Chad Greene 2012
inH2O = kPa*4.01463;


 function R = Robject(kappa, varargin)
%function R = Robject(kappa, [options])
%|
%| Build roughness penalty regularization "object" based on C = Cdiff() object,
%| for regularized solutions to inverse problems.
%|
%| General form of nonquadratic penalty function:
%|	R(x) = \sumk w_k \pot([Cx]_k), where [Cx]_k = \sum_j c_{kj} x_j.
%|
%| For quadratic case, \potk(t) = t^2/2 in which case
%|	R(x) = x' C' W C x / 2, where W depends on beta and edge_type.
%|
%| Penalty gradient is C' D C x, where D = diag{\wpot_k([Cx]_k)}.
%|
%| in
%|	kappa	[nx,ny[,nz]]	kappa array, or logical support mask
%|
%| options
%|	edge_type '?'		how to handle mask edge conditions (see Cdiff)
%|		'none'		no roughness penalty (NOT DONE)
%|		'tight'		only penalize within-mask differences (default)
%|		'leak'		penalize between mask and neighbors
%|				(its primary use for consistency with ASPIRE)
%|	'order', ?		1st-order or 2nd-order differences (see Cdiff)
%|	'offsets', [?]		offsets to neighboring pixels
%|					(see Cdiff for the defaults)
%|				use '3d:26' to penalize all 13 pairs of nbrs
%|				use '0' for C = I (identity matrix)
%|	'beta', ?		global regularization parameter
%|					default: 2^0
%|	'delta', ?		potential parameter, see potential_func()
%|					or {delta, param}.  default: inf
%|	'potential', '?'	e.g., 'huber', see potential_func()
%|				default: 'quad' for quadratic regularization.
%|	'type_denom', '?'	type of "denominator"
%|					(for quadratic surrogates like SPS)
%|		'matlab'	denominator for SPS
%|		'aspire'	denominator for SPS that matches aspire
%|		'none'		no denominator (default)
%|				(because R.E and R.denom needed only for SPS)
%|	'distance_power', ?	1 classical (default), 2 possibly improved
%|					see penalty_mex()
%|	'user_wt', [?]		User-provided array of penalty weight values
%|					size: [nx,ny[,nz],#offsets]
%|				of dimension [size(mask) length(offsets)].
%|				These are .* the usual wt values for edge_type.
%|	'mask'			Override default: mask = (kappa ~= 0)
%|
%| out
%|	R structure has the following "methods"
%|	R.penal(R, x)	evaluates R(x)
%|	R.cgrad(R, x)	evaluates \cgrad R(x) (column gradient)
%|	R.denom(R, x)	evaluates denominator for separable surrogate
%|	[pderiv pcurv] = feval(R.dercurv, R, C1*x) derivatives and curvatures
%|			for non-separable parabola surrogates
%|	R.diag(R)	diagonal of Hessian of R (at x=0), for preconditioners.
%|	R.C1		differencing matrix, with entries 1 and -1,
%|			almost always should be used in conjunction with R.wt
%|
%| Typical use:	mask = true([128 128]); % or something more conformal
%|		R = Robject(mask, 'beta', 2^7);
%|
%| Copyright 2004-11-14, Jeff Fessler, University of Michigan

if nargin < 1, ir_usage, end
if streq(kappa, 'test'), run_mfile_local 'Robject_test', return, end

% option defaults
R.potential = 'quad';
R.beta = 2^0;
R.delta = inf;
R.edge_type = 'tight';
R.type_denom = 'none';
R.distance_power = 1;
R.order = 1; % 1st-order differences, used in call to Cdiff
R.user_wt = []; % user-provided wt values
R.mask = [];
R.offsets = [];

% parse name/value option pairs
R = vararg_pair(R, varargin);

% dimensions, and default offsets
if isempty(R.offsets)
	if ndims(kappa) == 2
		[nx ny] = size(kappa);
		R.offsets = [1 nx nx+1 nx-1];
	elseif ndims(kappa) == 3
		[nx ny nz] = size(kappa);
		R.offsets = [1 nx nx*ny];
	else
		error 'only 2D and 3D done'
	end
elseif streq(R.offsets, '3d:26') % all 26 neighbors (13 pairs)
	[nx ny nz] = size(kappa);
	R.offsets = [1 nx+[0 1 -1] nx*ny+col(outer_sum([-1:1],[-1:1]*nx))'];
end

if ~iscell(R.delta)
	R.delta = {R.delta};
end

R.offsets = int32(R.offsets);

if streq(R.edge_type, 'none')
	error 'todo: unpenalized'
end

if R.beta < 0, warn('Negative beta? This is probably wrong!'), end

R.isquad = streq(R.potential, 'quad');

if isempty(R.mask)
	R.mask = kappa ~= 0; % default is to infer from kappas
end
if ~islogical(R.mask), error 'mask must be logical', end

%
% build plain sparse differencing object "C1", containing 1 and -1 values
% (or identity matrix)
%
R.C_is_I = any(R.offsets == 0);
if R.C_is_I
	if any(R.offsets), error 'identity is offsets=0', end
	R.C1 = diag_sp(ones(sum(R.mask(:)),1)); % identity matrix
	R.wt = R.beta;

else
	R.C1 = Cdiff(R.mask, 'offsets', R.offsets, ...
			'edge_type', 'none', 'order', R.order);

	% wk factors
	R.wt_string = sprintf('wk,%s,%d', R.edge_type, R.order);
	R.wt = penalty_mex(R.wt_string, single(kappa), R.offsets, R.distance_power);
	R.wt = R.beta * single(R.wt(:)); % absorb beta into wk factors
end

% incorporate user provided wk values
if ~isempty(R.user_wt)
	R.wt = single(R.wt .* R.user_wt(:));
	R.user_wt = []; % save memory
end

% trick: for quadratic case, provide a R.C object where C = sqrt(W) * C1
if R.isquad
	R.C = R.C1;
	R.C.cascade_after = diag_sp(sqrt(R.wt));
end

%
% desired potential function
%
R.pot = potential_func(R.potential, R.delta{:});

%
% functions
%
R.dercurv = @Robject_dercurv;
R.handle_denom = @Robject_denom;
R.handle_diag = @Robject_diag;
% trick: the following form of R.penal allows multiple realizations of x
R.penal = @(R, x) ... % 2014-04-27 added 'double' to better match new version
	sum(repmat(R.wt, ncol(x)) .* R.pot.potk(R.pot, R.C1 * x), 'double');
R.cgrad = @(R, x) R.C1' * (diag_sp(R.wt) * R.pot.dpot(R.pot, R.C1 * x));
R.diag = @(R, x) R.handle_diag(R);
R.denom = @(R, x) R.handle_denom(R, x);

%
% precompute denominator for separable quadratic case if needed
%
if ~streq(R.type_denom, 'none') && R.isquad
	if streq(R.type_denom, 'matlab') || streq(R.type_denom, 'aspire')
		t = R.wt .* R.pot.wpot(R.pot, 0);
		if R.C_is_I
			R.denom_max0 = t;
		else
			t = reshape(t, [size(R.mask) length(R.offsets)]);
			if R.order == 1 % fix: order=2 denom?
				t = 2 * t; % "2" because (1,-1) differences
				R.denom_max0 = penalty_mex('diff1,back2', ...
						single(t), R.offsets);
			elseif R.order == 2 % (-1,2,1) so ?
				t = 4 * t; % 4 = |-1| + |2| + |-1|
				R.denom_max0 = penalty_mex('diff2,backA', ...
						single(t), R.offsets);
			else
				error 'order not done for denom'
			end
			R.denom_max0 = single(R.denom_max0(R.mask));
		end
	else
		error(['Unknown type_denom: ' R.type_denom])
	end
end

%
% Robject_denom()
% jth penalty separable surrogate curvature is d_j = \sumk |\ckj| \ck \wpotk
% where \ck = \sumj |\ckj|.
% Here, ck = 2 since there is a +1 and a -1 per row of C1 (for 1st-order)
% Also, |\ckj| = |\ckj|^2 since \ckj = +/- 1, so we can use 'diff1,back2'
%
function denom = Robject_denom(R, x)
if streq(R.type_denom, 'none')
	error 'denom not initialized'
end

if R.isquad
	denom = R.denom_max0;
else
	t = single(R.wt .* R.pot.wpot(R.pot, R.C1 * x));
	if R.C_is_I
		denom = single(t);
	else
		t = reshape(t, [size(R.mask) length(R.offsets)]);
		if R.order == 1
			t = 2 * t; % because (1,-1)
			denom = penalty_mex('diff1,back2', t, R.offsets);
		elseif R.order == 2 % (-1,2,1) so ck = 4
			t = 4 * t; % 4 = |-1| + |2| + |-1|
			denom = penalty_mex('diff2,backA', ...
					single(t), R.offsets);
		else
			error 'order not done'
		end
		denom = single(denom(R.mask));
	end
end

%
% evaluate diagonal of Hessian of (quadratic surrogate for) R at x=0
% \sum_k w_k |c_kj|^2 \wpot(0)
%
function rjj = Robject_diag(R)
t = single(R.wt .* R.pot.wpot(R.pot, 0));
t = reshape(t, [size(R.mask) length(R.offsets)]);
if R.C_is_I
	error 'not done, ask jeff'
else
	rjj = penalty_mex('diff1,back2', t, R.offsets);
	rjj = double(rjj(R.mask));
end

%
% evaluate \dpoti and \wpoti
%
function [deriv, curv] = Robject_dercurv(R, C1x)
deriv = R.wt .* R.pot.wpot(R.pot, C1x) .* C1x;
curv = R.wt .* R.pot.wpot(R.pot, C1x);

%
% Compute both cgrad and denom (of separable surrogate) efficiently.
% Unused for now, but could be used if anonymous functions are too inefficient
% since both R.cgrad and R.denom use C*x so there is redundancy.
% What we really need is an anonymous function that has two output arguments.
% No, the feval with a function_handle will suffice!
%
%function [cgrad, denom] = Robject_cgrad_denom(R, x)
%if R.isquad
%	cgrad = R.C' * (R.C * x);
%	denom = R.denom;
%else
%	Cx = R.C * x;
%	wx = R.wt .* R.pot.wpot(R.pot, Cx);
%	cgrad = R.C' * (wx .* Cx);
%	denom = R.E * wx;
%end


function [oz] = cm32oz(cm3)
% Convert volume from cubic centimeters to US liquid ounces.
% Chad Greene 2012
oz = cm3*0.033814022701;


%  INTERNAL FUNCTION: computes chebyshev distances
%
%  ::
%
%    c=chebyshev_distance(y)
%
%  Args:
%
%     - **y** [numeric] : N x T x G array, with
%
%       - **N** [numeric] : number of simulations/replications
%       - **T** [numeric] : sample length (time series dimension)
%       - **G** [numeric] : number of variables
%
%  Returns:
%     :
%
%     - **c** [N x 1 vector] : chebyshev distances
%
%  See also:
%      - standardized_distance
%      - multivariate_chebyshev_box
%
%


function [t,x_new,f_new,g_new,funEvals,H] = ArmijoBacktrack(...
    x,t,d,f,fr,g,gtd,c1,LS_interp,LS_multi,progTol,debug,doPlot,saveHessianComp,funObj,options,varargin)
% [t,x_new,f_new,g_new,funEvals,H] = ArmijoBacktrack(...
%    x,t,d,f,fr,g,gtd,c1,LS_interp,LS_multi,progTol,debug,doPlot,saveHessianComp,funObj,varargin)
%
% Backtracking linesearch to satisfy Armijo condition
%
% Inputs:
%   x: starting location
%   t: initial step size
%   d: descent direction
%   f: function value at starting location
%   fr: reference function value (usually funObj(x))
%   gtd: directional derivative at starting location
%   c1: sufficient decrease parameter
%   debug: display debugging information
%   LS_interp: type of interpolation
%   progTol: minimum allowable step length
%   doPlot: do a graphical display of interpolation
%   funObj: objective function
%   varargin: parameters of objective function
%
% Outputs:
%   t: step length
%   f_new: function value at x+t*d
%   g_new: gradient value at x+t*d
%   funEvals: number function evaluations performed by line search
%   H: Hessian at initial guess (only computed if requested)
%
% recet change: LS changed to LS_interp and LS_multi

% Evaluate the Objective and Gradient at the Initial Step
if nargout == 6
    [f_new,g_new,H] = funObj(x + t*d,varargin{:});
else

  x_struct = vector2struct(x+t*d, options.TEMPLATE);
  [f_new,g_new_struct] = feval(funObj, x_struct,varargin{:});
  g_new = struct2vector(g_new_struct);

end
funEvals = 1;

while f_new > fr + c1*t*gtd || ~isLegal(f_new)
    temp = t;

    if LS_interp == 0 || ~isLegal(f_new)
        % Ignore value of new point
        if debug
            fprintf('Fixed BT\n');
        end
        t = 0.5*t;
    elseif LS_interp == 1 || ~isLegal(g_new)
        % Use function value at new point, but not its derivative
        if funEvals < 2 || LS_multi == 0 || ~isLegal(f_prev)
            % Backtracking w/ quadratic interpolation based on two points
            if debug
                fprintf('Quad BT\n');
            end
            t = polyinterp([0 f gtd; t f_new sqrt(-1)],doPlot,0,t);
        else
            % Backtracking w/ cubic interpolation based on three points
            if debug
                fprintf('Cubic BT\n');
            end
            t = polyinterp([0 f gtd; t f_new sqrt(-1); t_prev f_prev sqrt(-1)],doPlot,0,t);
        end
    else
        % Use function value and derivative at new point

        if funEvals < 2 || LS_multi == 0 || ~isLegal(f_prev)
            % Backtracking w/ cubic interpolation w/ derivative
            if debug
                fprintf('Grad-Cubic BT\n');
            end
            t = polyinterp([0 f gtd; t f_new g_new'*d],doPlot,0,t);
        elseif ~isLegal(g_prev)
            % Backtracking w/ quartic interpolation 3 points and derivative
            % of two
            if debug
                fprintf('Grad-Quartic BT\n');
            end
            t = polyinterp([0 f gtd; t f_new g_new'*d; t_prev f_prev sqrt(-1)],doPlot,0,t);
        else
            % Backtracking w/ quintic interpolation of 3 points and derivative
            % of two
            if debug
                fprintf('Grad-Quintic BT\n');
            end
            t = polyinterp([0 f gtd; t f_new g_new'*d; t_prev f_prev g_prev'*d],doPlot,0,t);
         end
    end

    % Adjust if change in t is too small/large
    if t < temp*1e-3
        if debug
            fprintf('Interpolated Value Too Small, Adjusting\n');
        end
        t = temp*1e-3;
    elseif t > temp*0.6
        if debug
            fprintf('Interpolated Value Too Large, Adjusting\n');
        end
        t = temp*0.6;
    end

    % Store old point if doing three-point interpolation
    if LS_multi
        f_prev = f_new;
        t_prev = temp;
        if LS_interp == 2
            g_prev = g_new;
        end
    end

    if ~saveHessianComp && nargout == 6
        [f_new,g_new,H] = funObj(x + t*d,varargin{:});
    else

      x_struct = vector2struct(x+t*d, options.TEMPLATE);
      [f_new,g_new_struct] = feval(funObj, x_struct,varargin{:});
      g_new = struct2vector(g_new_struct);

%         [f_new,g_new] = funObj(x + t*d,varargin{:});
    end
    funEvals = funEvals+1;

    % Check whether step size has become too small
    if max(abs(t*d)) <= progTol
        if debug
            fprintf('Backtracking Line Search Failed\n');
        end
        t = 0;
        f_new = f;
        g_new = g;
        break;
    end
end

% Evaluate Hessian at new point
if nargout == 6 && funEvals > 1 && saveHessianComp

  [f_new,g_new,H] = funObj(x + t*d,varargin{:});
  funEvals = funEvals+1;

end

x_new = x + t*d;

end


function [Btuph] = hp2Btuph(hp)
% Convert power from mechanical horsepower to British
% thermal units per hour.
% Chad A. Greene 2012
Btuph = hp*2544.433577644;


% system process model in local s0 frame with dcm formulation, Cs0e is the
% transformation between the sensor local s0 frame and e-frame
% written by Huai
% rqs02e, r s0 in e, q s0 2e, rvqs0, rs in s0, vs in s0, qs0 2s
% acc, m/s^2, acc by imu in s frame, gyro, rad/s, angular rate by imu
% w.r.t i frame coordinated in s frame, dt, time interval for covariance update
function [STM Qd]=sys_local_dcm_v000(rqs02e, rvqs0, acc, gyro, dt, imutype, modelNo)
% the covariance corresponds to states, rs0 in e, q s02e,
% rs in s0, v s in s0, q s02s, ba, bg, sa, sg, qs2c, Ts in c,
% for each group frame, qs02si and Tsi in s0, for each
% point, its inverse depth, here we are only interested up to sa and sg
% for modelNo=3 and imutype=5, MEMS
% acc bias drift, random walk
% gyro bias drift, random walk
% acc scale factor, random walk
% gyro scale factor, random walk

% for modelNo=1 and imutype=5 the acc and gyro turn on bias are removed
R=6317000; % earth average radius
%system disturbance coefs
N=zeros(15,6);
N(13:15,4:6)=eye(3); %attitude
Cs02s=quat2dcm_v000(rvqs0(7:10));
N(10:12,1:3)=Cs02s'; %velocity

%system matrix
A=zeros(15);
% 0s for r s0 in e and q s0 to e
A(7:9, 10:12)=eye(3); % rs in s0
%Velocity
xyz_imu=rqs02e(1:3)+quatrot_v000(rqs02e(4:7),rvqs0(1:3),0);
unit=xyz_imu/norm(xyz_imu,2);
Llh=ecef2geo_v000(xyz_imu,0);
[Rn, Re, g, sL, cL, WIE_E]=geoparam_v000(Llh);
geCoeff=-g*R^2/norm(xyz_imu,2)^3*(eye(3)-3*(unit*unit'))-skew([0;0;WIE_E])*skew([0;0;WIE_E]);
Cs02e=quat2dcm_v000(rqs02e(4:7));
A(10:12,1:3)=Cs02e'*geCoeff;
A(10:12,7:9)=Cs02e'*geCoeff*Cs02e;

Cne=pos2Cne_v000(Llh(1), Llh(2));
ge=Cne(:,3)*g-[xyz_imu(1:2);0]*WIE_E^2; % gravitation in e frame
A(10:12,4:6)=Cs02e'*(-skew(ge)+geCoeff*skew(quatrot_v000(rqs02e(4:7),rvqs0(1:3),0))+...
    skew(skew([0;0;WIE_E])*skew([0;0;WIE_E])*xyz_imu)-2*skew(quatrot_v000(rqs02e(4:7),rvqs0(4:6),0))*skew([0;0;WIE_E]));

A(10:12, 10:12)=-2*skew(quatrot_v000(rqs02e(4:7),[0;0;WIE_E], 1));
A(10:12, 13:15)=-Cs02s'*skew(acc);
%Attitude
A(13:15,4:6)=Cs02s*Cs02e'*skew([0;0;WIE_E]);
A(13:15,13:15)=skew(-gyro);
Anav=A;
Nnav=N;
% X(k+1) = ffun[X(k),U(k),V(k)]
% X(k+1) = Ak*X(k)+Gk*Vk

%%%%Imu error model parameters
[Aimu_d, Qimu_d, Cimu, Rimu]=imu_err_model_v001(acc, gyro, dt, imutype, modelNo);

%%%%Combine and discretize nav and imu models
% this discretization can also be accomplished by Loan's matrix exponential
% method, see sys_metric_phipsi_v000.m
navStates=15;
Anav_d=eye(navStates)+dt*Anav;  %Use 1st order taylor series to discretize Anav
Qnav=Nnav*Rimu*Nnav';
Qnav_d=dt/2*(Anav_d*Qnav+Qnav*Anav_d');      %Use trapezoidal rule to discretize Rimu

STM=zeros(navStates+size(Aimu_d,1));
STM(1:navStates,1:navStates)=Anav_d;
STM(1:navStates,1+navStates:end)=Nnav*Cimu*dt;
STM(1+navStates:end,1+navStates:end)=Aimu_d;

Qd=zeros(navStates+size(Aimu_d,1));
Qd(1:navStates,1:navStates)=Qnav_d;
Qd(1+navStates:end,1+navStates:end)=Qimu_d;
Qd(1:navStates,1+navStates:end)=Nnav*Cimu*Qimu_d*dt/2;
Qd(1+navStates:end,1:navStates)=Qd(1:navStates,1+navStates:end)';
end


%Simple function to test the cross method
d=10;
%elem_fun=@(x) sum(x); %Just sum of everything
p=0:d-1; p = 2.^p;
a=-5;b=5;
n=2^d;
h=(b-a)/(n-1);
%mv=@(x) x.^3;
mv=@(x) sqrt(x)+abs(x);
%elem_fun=@(x) 1.0./(dot((x-1),p)+1e-3); %Just sum of everything
%elem_fun=@(x) mv(1e-12+dot(x-1,p)*h);

%Compare functions of TT-tensors
x=tt_x(d,2); x=tt_tensor(x);
e=tt_ones(d,2);e=tt_tensor(e);
x=a*e+h*x; x=round(x,1e-13);
x1=kron(e,x); x2=kron(x,e);
rs=x1.^2+x2.^2; rs=round(rs,1e-13);
%fun=@(x) 1.0./sqrt(x);
%fun=@(x) 1.0./x;
fun=@(x) exp(-x.^4);
%fun = @(x) x.^2;
rs=x;
%elem_fun = @(ind) fun(rs(ind));
%elem_fun=@(ind) rs(ind);
%elem_fun=@(x) sqrt(x(1))+x(2);
elem_fun = @(ind) fun(rs(ind));
eps=1e-6;
D=ndims(rs);
%y=tt_rc2(2*d,2,elem_fun,1e-12);
y=tt_rc(D,2,elem_fun,1e-6,'nswp',40);
%v=tt_rand(size(y),ndims(y),2);
%y=y+v; y=round(y,1e-12);
%y1=tt_rc(D,2,elem_fun,1e-8,'nswp',40,'x0',y);

z=funcrs2(rs,fun,1e-6,rs,20);
z=round(z,1e-12);
z1=tt_rc(D,2,elem_fun,1e-8,'nswp',40,'x0',z);


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


%% manage paths

path_dict = manage_paths();

path2module = path_dict(1);
path2matlabcode_folder = path_dict(2);
path2input = path_dict(3);
path2output = path_dict(4);

%%
%Handover "mode", "another_map", "raceline_percentage_mode" & "raceline_percent_value" from Workspace
mode=evalin('base', 'mode');
another_map=evalin('base', 'another_map');
centerline_mode=evalin('base', 'centerline_mode');
raceline_percentage_mode=evalin('base', 'raceline_percentage_mode');
if raceline_percentage_mode==1
    raceline_percent_value=evalin('base', 'raceline_percent_value');
end
%Import Track file
if centerline_mode==1
    name=evalin('base','name');
    res_centerline_mode=evalin('base','resolution');
    track=importdata(strcat(path2output,'\tracks\Track_',name,'_',strrep((num2str(res_centerline_mode)),'.','-'),'.mat'));
    map_name=[name,'_',strrep((num2str(res_centerline_mode)),'.','-')];
else
    path_track=evalin('base','path_track');
    filename=evalin('base', 'filename');
    track=importdata(path_track);
    map_name=filename(7:end-4);
end

%Convert track to double
track=double(track);

%Get size of track_limits_filled file
pixelwidth_x=size(track,2);
pixelwidth_y=size(track,1);
if pixelwidth_x>=pixelwidth_y
    pixelwidth=pixelwidth_x;
else
    pixelwidth=pixelwidth_y;
end

%Extract origin of coordinates from track_limits_filled file
x_min_lim=track(1,1);
x_max_lim=track(1,2);
y_min_lim=track(2,1);
y_max_lim=track(2,2);

%Generate x-y-range array
x_lim=[x_min_lim x_max_lim];
y_lim=[y_min_lim y_max_lim];

%Generate parameter for random mode depending on size of track_limits_filled.pgm file
rand_n=round(pixelwidth/113)+1;

%Extract resolution of Map
resolution=track(1,3)/100;

%Generate origin of coordinates Array
limits=[x_min_lim x_max_lim ; y_min_lim y_max_lim ];

%Delete origin information
track(1:2,1:3)=0;
%Calculate raceline mode "normal" and "percentage"
if strcmp(mode,'raceline') || raceline_percentage_mode ==1
        %Handover Raceline Path from base workspace
        path_raceline=evalin('base','path_raceline');
        %Read in Raceline
        fid=fopen(path_raceline,'r');
        exportraceline=textscan(fid, '%f %f %f %f %f %f %f', 'Headerlines', 3, 'delimiter', ';');
        %exportraceline=textscan(fid, '%f %f', 'Headerlines', 2, 'delimiter', ';');
        fclose(fid);
        %Separate Columns of Raceline file, short file and get size
        x_0_race=exportraceline{:,2};
        y_0_race=exportraceline{:,3};
        intervall=round(size(x_0_race,1)/300);
        x_0_race=x_0_race(1:intervall:end,:);
        y_0_race=y_0_race(1:intervall:end,:);
        height=size(x_0_race,1);
        %Calculate gradients and normals of all points
        m_tang_race=gradient(y_0_race,x_0_race);
        m_normal_race= (-1./m_tang_race);
        %Calculate X and Y-component of displacement of 2 meters
        x_komp_2m=sqrt((2^2)./((1+m_normal_race.^2)));
        y_komp_2m=m_normal_race.*x_komp_2m;
        %Get variables from base workspace
        if strcmp(mode,'raceline')
            raceline_value_1=evalin('base', 'raceline_value_1');
            raceline_value_2=evalin('base', 'raceline_value_2');
        elseif raceline_percentage_mode==1
            raceline_value_1=1+raceline_percent_value/100;
            if raceline_value_1<1
                raceline_value_2=-0.03;
            else
                raceline_value_2=0.03;
            end
        end
        %Calculate new coordinates with distance of 0m, 4m and 10 meters
        xyz_0m=[x_0_race y_0_race raceline_value_1*ones(height,1)];
        xyz_4m_plus= [x_0_race+2*(x_komp_2m) y_0_race+2*(y_komp_2m) raceline_value_1*exp(-raceline_value_2*0.1*4^2)*ones(height,1)];
        xyz_4m_minus= [x_0_race-2*(x_komp_2m) y_0_race-2*(y_komp_2m) raceline_value_1*exp(-raceline_value_2*0.1*4^2)*ones(height,1)];
        xyz_10m_plus= [x_0_race+5*(x_komp_2m) y_0_race+5*(y_komp_2m) raceline_value_1*exp(-raceline_value_2*0.1*10^2)*ones(height,1)];
        xyz_10m_minus= [x_0_race-5*(x_komp_2m) y_0_race-5*(y_komp_2m) raceline_value_1*exp(-raceline_value_2*0.1*10^2)*ones(height,1)];
        xyz=[xyz_0m;xyz_4m_plus;xyz_4m_minus;xyz_10m_plus;xyz_10m_minus];

        %Calculate conversion factors coordinates <-> pixels
        x_change=(pixelwidth_x/(abs(x_lim(1))+abs(x_lim(2))));
        y_change=(pixelwidth_y/(abs(y_lim(1))+abs(y_lim(2))));

        %Generate intervall in coordination space
        x_intervall=linspace(x_change*x_lim(1),x_change*x_lim(2),pixelwidth_x);
        y_intervall=linspace(y_change*y_lim(1),y_change*y_lim(2),pixelwidth_y);
        y_intervall=y_intervall(:);

        %Interpolate points in mesh
        [X,Y,Z]= griddata(x_change*xyz(:,1), y_change*xyz(:,2), xyz(:,3),x_intervall, y_intervall, 'cubic');
        Z=flipud(Z);
        %if raceline mode
        if strcmp(mode,'raceline')
            Z(isnan(Z))=0;
            Z(Z<0)=0;
        %if raceline percentage mode
        elseif raceline_percentage_mode==1
            Z(isnan(Z))=1;
            if raceline_value_1>1
                Z(Z<1)=1;
            else
                Z(Z>1)=1;
            end
        end
end

%Handover of other variables from Workspace depending on mode
%Insert origin of coordinates information and save file
currentFolder=cd;
switch mode
    case 'global'
         global_value=evalin('base', 'global_value');
         result=track.*global_value*100;
         if raceline_percentage_mode==1
             result=result.*Z;
         end
         result=int16(round(result));
         surf(result,'EdgeColor','none');
         set(gca,'YDir', 'reverse');
         set(gca,'XLim',[0, pixelwidth_x],'XTick',[0 pixelwidth_x]);
         set(gca,'YLim',[0, pixelwidth_y],'YTick',[0 pixelwidth_y]);
         view([0 90]);
         caxis([0 global_value*100+20]);
         colorbar
         result(1:2,1:2)=limits;
         result(1,3)=resolution*100;
         if raceline_percentage_mode==1
%             save([currentFolder,'\frictionmap_tools\matlab_code\outputs\maps\Map_',map_name,'Meter_Global_',strrep((num2str(global_value)),'.','-'),'_Raceline_', strrep((num2str(raceline_percent_value)),'.','-'),'_Percent.mat'], 'result');
            save([currentFolder,'\variable_friction\generate_map\outputs\maps\Map_',map_name,'Meter_Global_',strrep((num2str(global_value)),'.','-'),'_Raceline_', strrep((num2str(raceline_percent_value)),'.','-'),'_Percent.mat'], 'result');
         else
%             save([currentFolder,'\frictionmap_tools\matlab_code\outputs\maps\Map_',map_name,'Meter_Global_',strrep((num2str(global_value)),'.','-'),'.mat'], 'result');
            save([currentFolder,'\variable_friction\generate_map\outputs\maps\Map_',map_name,'Meter_Global_',strrep((num2str(global_value)),'.','-'),'.mat'], 'result');
         end
    case 'random'
         random_value_1=evalin('base', 'random_value_1');
         random_value_2=evalin('base', 'random_value_2');
         A=rand(rand_n);
         B=A.*(random_value_2-random_value_1)+random_value_1;
         C=interp2(B,7,'cubic',0);
         D=C(1:pixelwidth_y,1:pixelwidth_x);
         result=D.*track*100;
         if raceline_percentage_mode==1
             result=result.*Z;
         end
         result=int16(round(result));
         surf(result,'EdgeColor','none');
         set(gca,'YDir', 'reverse');
         set(gca,'XLim',[0, pixelwidth_x],'XTick',[0 pixelwidth_x]);
         set(gca,'YLim',[0, pixelwidth_y],'YTick',[0 pixelwidth_y]);
         caxis([random_value_1*100-20 random_value_2*100+20]);
         colorbar
         view([0 90]);
         result(1:2,1:2)=limits;
         result(1,3)=resolution*100;
         if raceline_percentage_mode==1
             save(strcat(path2output,'\maps\Map_',map_name,'Meter_Random_',strrep((num2str(random_value_1)),'.','-'),'_',strrep((num2str(random_value_2)),'.','-'),'_Raceline_', strrep((num2str(raceline_percent_value)),'.','-'),'_Percent.mat'), 'result');
         else
             save(strcat(path2output,'\maps\Map_',map_name,'Meter_Random_',strrep((num2str(random_value_1)),'.','-'),'_',strrep((num2str(random_value_2)),'.','-'),'.mat'), 'result');
         end
    case 'raceline'
        result=Z.*track*100;
        result=int16(round(result));
        surf(result,'EdgeColor','none');
        set(gca,'YDir', 'reverse');
        set(gca,'XLim',[0, pixelwidth_x],'XTick',[0 pixelwidth_x]);
        set(gca,'YLim',[0, pixelwidth_y],'YTick',[0 pixelwidth_y]);
        view([0 90]);
        caxis([0 raceline_value_1*100]);
        colorbar
        result(1:2,1:2)=limits;
        result(1,3)=resolution*100;
        save([path2output, '\maps\Map_',map_name,'Meter_Raceline_',strrep((num2str(raceline_value_1)),'.','-'),'_',strrep((num2str(raceline_value_2)),'.','-'),'.mat'], 'result');
end
%If another_map==1, run Read_FrictionmodeGUI again
if another_map==1
    Read_FrictionmodeGUI();
    clc
    "Friction Coefficient Map(s) generated successfully!" %#ok<NOPTS>
elseif another_map==0
    clc
    "Friction Coefficient Map(s) generated successfully!" %#ok<NOPTS>
    evalin('base','clear');
end


function f=maxdiag(x,varargin)
% Maximizaton of the diagonality of hte matrix.
covmat = varargin{1};   % #pix X #pix X #taus
sizevec = varargin{2};
W = reshape(x, sizevec(1)*sizevec(2), sizevec(3)); % #pix X #comp
% W = max(W,eps);
ntau = size(covmat,3); % #taus (different time delays)
ftmp = zeros(1,ntau);
fcol = zeros(1,sizevec(3));
for tau=1:ntau
    covmattmp = covmat(:,:,tau);
    for col=1:sizevec(3)
        wcol = W(:,col);
        fcol(col) = log(wcol'*covmattmp*wcol);
    end
    ftmp(tau) = 0.5*sum(fcol);
end

f = sum(ftmp);


function [ x, know ] = p02_sol ( m, know )

%*****************************************************************************80
%
%% P02_SOL returns known solutions for problem 2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Harald Niederreiter, Kevin McCurley,
%    Optimization of functions by quasi-random search methods,
%    Computing,
%    Volume 22, Number 2, 1979, pages 119-123.
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input/output, integer KNOW.
%    On input, KNOW is 0, or the index of the previously returned solution.
%    On output, KNOW is 0 if there are no more solutions, or it is the
%    index of the next solution.
%
%    Output, real X(M), the solution.
%
  if ( know == 0 )
    know = 1;
    x = [ ...
      0.390500591228663, ...
      0.392051909813608, ...
      0.393601661544812, ...
      0.395149843840982 ]';
  else
    know = 0;
    x = zeros ( m, 1 );
  end

  return
end


function [x,state] = struct_transpose(z,task)
%STRUCT_TRANSPOSE Transpose.
%   [x,state] = struct_transpose(z) computes x as the transpose of z. The
%   structure state stores information which is reused in computing the
%   right and left Jacobian-vector products.
%
%   struct_transpose(z,task) computes the right or left Jacobian-vector
%   product of this transformation, depending on the structure task. Use
%   the structure state and add the field 'r' of the same shape as z or the
%   field 'l' of the same shape as x to obtain the structure task for
%   computing the right and left Jacobian-vector products
%
%      (dF(:)/dz(:).')*task.r(:) and
%      (dF(:)/dz(:).')'*task.l(:) + conj((dF(:)/dconj(z(:)).')'*task.l(:)),
%
%   respectively. Here, F(z) represents this transormation, (:) signifies
%   vectorization and the derivative w.r.t. z (conj(z)) is a partial
%   derivative which treats conj(z) (z) as constant. The output has the
%   same shape as x or z for the right and left Jacobian-vector products,
%   respectively.
%
%   See also struct_conj, struct_ctranspose.

%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
%            Marc Van Barel (Marc.VanBarel@cs.kuleuven.be)
%            Lieven De Lathauwer (Lieven.DeLathauwer@kuleuven-kulak.be)
%
%   References:
%   [1] L. Sorber, M. Van Barel, L. De Lathauwer, "Structured data fusion,"
%       ESAT-SISTA Internal Report 13-177, KU Leuven, 2013.

if nargin < 2, task = []; end
state = [];

if isempty(task) || (isempty(task.l) && isempty(task.r))
    x = z.';
elseif ~isempty(task.r)
    x = task.r.';
elseif ~isempty(task.l)
    x = task.l.';
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


function double_c_plot ( x, y, c )

%*****************************************************************************80
%
%% DOUBLE_C_PLOT displays the "double C" data.
%
%  Discussion:
%
%    The data comprises a "C" shape and its reverse, which do not intersect,
%    but which nestle together fairly closely.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%   Input, real X(N), Y(N), the coordinates of the data points.  The points
%   have been shuffled so that data belonging to the two components is
%   interleaved.
%
%   Input, integer C(N), is 1 or 2, depending on which set the corresponding
%   data point belongs to.
%
  i1 = ( c == 1 );
  i2 = ( c == 2 );

  n1 = sum ( i1 );
  n2 = sum ( i2 );

  plot ( x(i1), y(i1), 'r.', ...
         x(i2), y(i2), 'b.' )

  xlabel ( '<-- X -->' );
  ylabel ( '<-- Y -->' );
  title ( sprintf ( 'Double C Data, N1 (red) = %d, N2 (blue) = %d', n1, n2 ) );
  grid on
  axis equal

  return
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


function test_failed=test_spread
%TEST_SPREAD  Test spreading function.
%
%  This script runs a throrough test of the SPREADOP routine,
%  testing it on a range of input parameters.
global LTFAT_TEST_TYPE;
disp(' ===============  TEST_SPREAD ================');

disp('--- Used subroutines ---');

which comp_col2diag

Lr=[12, 13 ];

test_failed=0;

condNoLim.('single') = 100;
condNoLim.('double') = 100;

for ii=1:length(Lr);

L=Lr(ii);

spfraction=.5;

for rtype=1:2

  if rtype==1
    rname='REAL ';
  else
    rname='CMPLX';
  end;

  for sptype=1:2

    if sptype==1
      spname='FULL  ';
    else
      spname='SPARSE';
      if ~strcmpi(LTFAT_TEST_TYPE,'double')
          disp(sprintf('Skipping. Cannot work with sparse matrices of type %s.',LTFAT_TEST_TYPE));
          break;
      end
    end;

    condNo = 1e10;
    while condNo>condNoLim.(LTFAT_TEST_TYPE)
    if rtype==1
      if sptype==1
        coef=tester_rand(L,L);
        T=tester_rand(L,L);
        coef2=tester_rand(L,L);
      else
        coef=tester_sprand(L,L,spfraction);
        T=tester_sprand(L,L,spfraction);
        coef2=tester_sprand(L,L,spfraction);
      end;
    else
      if sptype==1
        coef=tester_crand(L,L);
        T=tester_crand(L,L);
        coef2=tester_crand(L,L);
      else
        coef=tester_sprand(L,L,spfraction);
        T=tester_sprand(L,L,spfraction);
        coef2=tester_sprand(L,L,spfraction);
      end;
    end;

      coeftmp=ifft(full(coef))*L;
      % The following matrix is inverted in spreadinv. We want a nicer cond
      % number in order not to fail
      Ttmp=comp_col2diag(coeftmp);
      condNo = cond(Ttmp);
    end

    for W=1:3

      if rtype==1
        f=tester_rand(L,W);
      else
        f=tester_crand(L,W);
      end;

      % ---------- Reference testing ------------

      fs=spreadop(f,coef);
      fs2=ref_spreadop(f,coef,1);

      fsdiff=fs-fs2;
      res=norm(fsdiff(:));
      [test_failed,fail]=ltfatdiditfail(res,test_failed);
      s=sprintf('REF %s %s L:%3i W:%2i %0.5g %s',rname,spname,L,W,res,fail);
      disp(s)

      % -------- Inversion testing -------------

      r=spreadinv(fs,coef);

      rdiff=f-r;
      res=norm(rdiff(:));
      [test_failed,fail]=ltfatdiditfail(res,test_failed);
      s=sprintf('INV %s %s L:%3i W:%2i %0.5g %s',rname,spname,L,W,res,fail);
      disp(s)

      % -------- Twisted convolution -------------------

      f1=spreadop(spreadop(f,coef2),coef);
      f2=spreadop(f,tconv(coef,coef2));

      rdiff=f1-f2;
      res=norm(rdiff(:));
      [test_failed,fail]=ltfatdiditfail(res,test_failed);
      s=sprintf('TWI %s %s L:%3i W:%2i %0.5g %s',rname,spname,L,W,res,fail);
      disp(s)

      % -------- Spreading function ---------------------

      coef=spreadfun(T);

      rdiff=T*f-spreadop(f,coef);

      res=norm(rdiff(:));
      [test_failed,fail]=ltfatdiditfail(res,test_failed);
      s=sprintf('FUN %s %s L:%3i W:%2i %0.5g %s',rname,spname,L,W,res,fail);
      disp(s)

      % -------- Adjoint operator -----------------------

      cadj=spreadadj(coef);

      rdiff=T'*f-spreadop(f,cadj);

      res=norm(rdiff(:));
      [test_failed,fail]=ltfatdiditfail(res,test_failed);
      s=sprintf('ADJ %s %s L:%3i W:%2i %0.5g %s',rname,spname,L,W,res,fail);
      disp(s)

    end;

  end;

end;
end;


% Dynamical model function for the random sine signal demo

% Copyright (C) 2007 Jouni Hartikainen
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.

function x_n = ekf_sine_f(x,param)
    dt = param(1);
    A = [1 dt 0;0 1 0;0 0 1];
    x_n = A*x(1:3,:);
    if size(x,1) == 7 || size(x,1) == 6
        x_n(1:3,:) = x_n(1:3,:) + x(4:6,:);
    end


function ac = lpcweight(ar,c)
%  lpcweight --> LPC based perceptual weighting filter.
%
%    ac = lpcweight(ar,c)
%
%    The function takes the LP coefficients, ar = [1 -a(1) ... -a(M)],
%    and the parameter, c, as inputs, and returns the coefficients of
%    the filter function A(z/c) in the vector ac.

% Linear predictor order.
M = length(ar);

% The i'th coefficient of A(z/c) is given by ar(i)*c^(i-1).
ac = ar;
ci = c;
for (i=2:M)
  ac(i) = ar(i)*ci;
  ci = ci*c;
end


function fem=springs(fem,iter);
% SPRING is legacy code that moves internal points of a finite
%  element mesh such that the mesh quality is improved.  The movement
%  of nodes is accomplished with a Laplacian smoothing action, so
%  that each point not located along the boundary is moved toward
%  the center of mass of the polygon formed by the adjacent
%  triangles. The process is repeated according to the setting
%  of the ITER variable.
%
%       Calls laplace_smooth.m with specified input.
% FileName:  springs.m
% Written By:  unknown
% Date Last Modified:
%    May 9, 2007 -- Chris Massey, NRL Code 7322, Stennis Spc Cnt., MS
%                   Deleted code and replaced with a call to
%                   laplace_smooth.m.
%    May 15, 2007 -- Chris Massey, -- Fixed bug, should send over the
%                   node list not the element list.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin==1,
    iter=5;
end

fem = laplace_smooth(fem,1:length(fem.x),iter);

return


function gT = simKernDiagGradX(kern, t);

% SIMKERNDIAGGRADX Gradient of SIM kernel's diagonal with respect to the
% input times t.
% FORMAT
% DESC computes the gradient of the diagonal of the single input motif
% kernel matrix with respect to the elements of the design matrix given
% in t.
% ARG kern : the kernel structure for which gradients are being computed.
% ARG t : the input data in the form of a design matrix.
% RETURN gT : the gradients of the diagonal with respect to each element
% of t. The returned matrix has the same dimensions as t.
%
% SEEALSO : simKernParamInit, kernDiagGradX, simkernGradX
%
% COPYRIGHT : Neil D. Lawrence, 2006
%
% COPYRIGHT : David Luengo, 2009

% KERN

if size(t, 2) > 1
  error('Input can only have one column');
end

sigma = sqrt(2/kern.inverseWidth);
t = t - kern.delay;
halfSigmaD = 0.5*sigma*kern.decay;

gT = zeros(size(t));

if (kern.isStationary == false)
    lnPart1 = lnDiffErfs(halfSigmaD, halfSigmaD-t/sigma);
    gT = kern.variance * exp(halfSigmaD*halfSigmaD - 2*kern.decay*t + lnPart1);
else
    gT = zeros(size(t));
end

if ~isfield(kern, 'isNormalised') || (kern.isNormalised == false)
    gT = gT * sigma * sqrt(pi);
end

if isfield(kern, 'gaussianInitial') && kern.gaussianInitial,
  error('simKerDiagGradX not implemented for gaussianInitial')
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');


% TEST_MAXWELL_RING_H_DRCHLT: data function for Dirichlet boundary condition.

function h = test_maxwell_ring_h_drchlt (x, y, ind)

  [theta, r] = cart2pol (x, y);
  h = zeros (size(x));
  switch (ind)
    case 1
      h = -sin(theta) .* sin(y) + cos(theta) .* sin(x);
    case 2
      h = sin(theta) .* sin(y) - cos(theta) .* sin(x);
    case 3
      h = -sin(y);
    case 4
      h = sin(x);
    otherwise
      error ('h_drchlt: unknown reference number')
  end

end


% 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;