  function ob = block_fatrix(blocks, varargin)
%|function ob = block_fatrix(blocks, options)
%|
%| Construct block_fatrix object, a meta-Fatrix composed of Fatrix blocks,
%| such as block_diag(A_1, A_2, ..., A_M)
%| See block_fatrix_test.m for example usage.
%|
%| in
%|	blocks	{cell}	cell array of the blocks
%|
%| options
%|	'type'	char	options:
%|				'diag' for block diagonal (default)
%|				'col' for [A_1; A_2; ...; A_M]
%|				'kron' for kron(eye(Mkron), blocks{1})
%|				'row' for [A1, A2, ..., A_M]
%|				'sum' for A1 + A2 + ... + A_M
%|	'Mkron'	int	required for 'kron' type
%|	'tomo'	0|1	special support for tomography-type objects (default: 0)
%|
%| out
%|	ob [nd np]	diag,row: np = sum_m ncol(A_m)
%|
%| Copyright 05-5-12, Jeff Fessler, University of Michigan

if nargin < 1, ir_usage, end
if nargin == 1 && streq(blocks, 'test')
	run_mfile_local block_fatrix_test
return
end

arg.blocks = blocks;

% defaults
arg.type = 'diag';
arg.chat = 0;
arg.tomo = 0;
arg.Mkron = [];

% options
arg = vararg_pair(arg, varargin);

% initialize
if ~iscell(blocks)
	fail('blocks must be cell array, not "%s"', class(blocks))
end

switch arg.type
case 'col'
	ob = block_fatrix_col(blocks, arg);
case 'diag'
	ob = block_fatrix_diag(blocks, arg);
case 'kron'
	ob = block_fatrix_kron(blocks, arg);
case 'row'
	ob = block_fatrix_row(blocks, arg);
case 'sum'
	ob = block_fatrix_sum(blocks, arg);
otherwise
	fail('unknown block type "%s"', arg.type)
end

%
% block_fatrix_col()
%
function ob = block_fatrix_col(blocks, arg)

MM = length(blocks);
arg.dims = zeros(MM, 2);
for ii=1:MM
	arg.dims(ii,:) = size(blocks{ii});
	if arg.dims(ii,2) ~= arg.dims(1,2)
		error 'all blocks must have same #cols for "col"'
	end
end
% start/end indices for selecting parts of x and y
arg.istart = cumsum([1; arg.dims(1:end-1,1)]);
arg.iend = arg.istart + arg.dims(:,1) - 1;

arg.dim = [sum(arg.dims(:,1)) arg.dims(1,2)];

if arg.tomo % prep for mat2cell later
	for ii=1:MM
		odim = arg.blocks{ii}.arg.odim;
		arg.tomo_ndim = length(odim);
		arg.tomo_dim = arg.tomo_ndim; % insist on last dimension
		if ii==1
			for id = 1:arg.tomo_ndim
				if id ~= arg.tomo_dim
					arg.mat2cell_arg{id} = odim(id);
				end
			end
		end
		arg.mat2cell_arg{arg.tomo_dim}(ii) = odim(arg.tomo_dim);
	end
end

% build Fatrix object
ob = Fatrix(arg.dim, arg, 'caller', 'block_fatrix(col)', ...
	'forw', @block_fatrix_col_forw, 'back', @block_fatrix_col_back, ...
	'free', @block_fatrix_free, 'gram', @block_fatrix_col_gram);

%
% block_fatrix_col_forw(): y = A * x
%
function y = block_fatrix_col_forw(arg, x)

MM = length(arg.blocks);
y = cell(MM,1);
for ii=1:MM
	y{ii} = arg.blocks{ii} * x;
end

if ~arg.tomo || ncol(y{1}) == 1 ...
	|| (arg.tomo && arg.dim(2) == size(x,1)) % [np (L)]
	y = cat(1, y{:});
else % handle 'tomo' case where x is not a single column
	y = cat(arg.tomo_dim, y{:});
end

%
% block_fatrix_col_back(): x = A' * y
%
function x = block_fatrix_col_back(arg, y)

MM = length(arg.blocks);
if arg.tomo
	if arg.dim(1) == size(y,1) % [nd (L)]
		arg.tomo = 0;
	else % [(odim) (L)] split into separate parts, undoing cat(?, y{:})
		tmp = size(y);
		if length(tmp) == arg.tomo_ndim
			yc = mat2cell(y, arg.mat2cell_arg{:});
		elseif length(tmp) > arg.tomo_ndim
			yc = mat2cell(y, arg.mat2cell_arg{:}, ...
				tmp((arg.tomo_ndim+1):end)); % handle (L)
		else
			error 'bug'
		end
	end
end

x = 0;
for ii=1:MM
	if ~arg.tomo % [nd (L)]
		jj = arg.istart(ii):arg.iend(ii);
		yi = y(jj,:);
	else
		yi = yc{ii};
	end
	t = arg.blocks{ii}' * yi;
	x = x + t;
end

%
% block_fatrix_col_gram()
%
function [T, reuse] = block_fatrix_col_gram(ob, W, reuse, varargin)

blocks = ob.arg.blocks;
T = cell(size(blocks));
for ii=1:length(blocks)
	A = blocks{ii};
	if isnumeric(A)
		if isempty(W)
			T{ii} = A' * A;
		else
			warn 'todo: this may not work, need piece of W!'
			T{ii} = A' * W * A;
		end
	else
		if isempty(W)
			T{ii} = build_gram(A, [], reuse, varargin{:});
		else
			if isvar('W.arg.blocks{ii}')
				T{ii} = build_gram(A, W.arg.blocks{ii}, ...
					reuse, varargin{:});
			else
				fail('block_fatrix_col_gram needs block diag W')
			end
		end
	end
end
T = fatrix_plus(T{:});

%
% block_fatrix_row()
% trick: just reuse col via transpose!
%
function ob = block_fatrix_row(blocks, arg)

for ii=1:length(blocks)
	blocks{ii} = blocks{ii}'; % trick: transpose
end

ob = block_fatrix(blocks, 'type', 'col')'; % trick: transpose

%
% block_fatrix_diag()
%
function ob = block_fatrix_diag(blocks, arg)

arg.dims = zeros(length(blocks), 2);
for ii=1:length(blocks)
	arg.dims(ii,:) = size(blocks{ii});
end
% start/end indices for selecting parts of x and y
arg.istart = cumsum([1; arg.dims(1:end-1,1)]);
arg.iend = arg.istart + arg.dims(:,1) - 1;
arg.jstart = cumsum([1; arg.dims(1:end-1,2)]);
arg.jend = arg.jstart + arg.dims(:,2) - 1;

arg.dim = sum(arg.dims, 1);

% build Fatrix object
ob = Fatrix(arg.dim, arg, 'caller', 'block_fatrix(diag)', ...
	'forw', @block_fatrix_diag_forw, 'back', @block_fatrix_diag_back, ...
	'free', @block_fatrix_free, 'gram', @block_fatrix_diag_gram, ...
	'mtimes_block', @block_fatrix_diag_mtimes_block);

%
% block_fatrix_diag_forw(): y = A * x
%
function y = block_fatrix_diag_forw(arg, x)

if nrow(x) ~= arg.dim(2)
	error('x size=%d vs dim(2)=%d', nrow(x), arg.dim(2))
end
y = [];
for ii=1:length(arg.blocks)
	t = arg.blocks{ii} * x([arg.jstart(ii):arg.jend(ii)], :);
	y = [y; t];
end

%
% block_fatrix_diag_back(): x = A' * y
% full backprojection
%
function x = block_fatrix_diag_back(arg, y)

if nrow(y) ~= arg.dim(1), error 'bad y size', end
x = [];
for ii=1:length(arg.blocks)
	t = arg.blocks{ii}' * y([arg.istart(ii):arg.iend(ii)], :);
	x = [x; t];
end

%
% block_fatrix_diag_mtimes_block()
% caution: it is quite unclear whether these are useful!
%
function y = block_fatrix_diag_mtimes_block(arg, is_transpose, x, istart, nblock)
if is_transpose
	y = block_fatrix_diag_block_back(arg, x, istart, nblock);
else
	y = block_fatrix_diag_block_forw(arg, x, istart, nblock);
end

%
% block_fatrix_diag_block_forw()
%
function y = block_fatrix_diag_block_forw(arg, x, istart, nblock)

if nrow(x) ~= arg.dim(2)
	error('x size=%d vs dim(2)=%d', nrow(x), arg.dim(2))
end
y = [];
for ii=istart:nblock:length(arg.blocks)
	t = arg.blocks{ii} * x([arg.jstart(ii):arg.jend(ii)], :);
	y = [y; t];
end

%
% block_fatrix_diag_block_back()
%
function x = block_fatrix_diag_block_back(arg, y, istart, nblock)

if nrow(y) ~= arg.dim(1), error 'bad y size', end
x = [];
for ii=istart:nblock:length(arg.blocks)
	t = arg.blocks{ii}' * y([arg.istart(ii):arg.iend(ii)], :);
	x = [x; t];
end

%
% block_fatrix_diag_gram()
%
function [T, reuse] = block_fatrix_diag_gram(ob, W, reuse, varargin)

blocks = ob.arg.blocks;
T = cell(size(blocks));
for ii=1:length(blocks)
	A = blocks{ii};
	if isnumeric(A)
		if isempty(W)
			T{ii} = A' * A;
		else
			T{ii} = A' * W * A;
		end
	else
		T{ii} = build_gram(A, W, reuse, varargin{:});
	end
end
T = block_fatrix(T, 'type', 'diag');

%
% block_fatrix_kron()
%
function ob = block_fatrix_kron(blocks, arg)

if isempty(arg.Mkron), error 'Mkron required', end
if length(blocks) ~= 1, error 'kron expects exactly one block', end

arg.dims = repmat(size(blocks{1}), [arg.Mkron 1]);
% start/end indices for selecting parts of x and y
arg.istart = cumsum([1; arg.dims(1:end-1,1)]);
arg.iend = arg.istart + arg.dims(:,1) - 1;
arg.jstart = cumsum([1; arg.dims(1:end-1,2)]);
arg.jend = arg.jstart + arg.dims(:,2) - 1;
arg.dim = sum(arg.dims,1);

% build Fatrix object
if isa(blocks{1}, 'Fatrix')
	tmp = ['F:' blocks{1}.caller];
else
	tmp = class(blocks{1});
end
tmp = sprintf('block_fatrix(kron, %s)', tmp);
ob = Fatrix(arg.dim, arg, 'caller', tmp, ...
	'forw', @block_fatrix_kron_forw, 'back', @block_fatrix_kron_back, ...
	'block_setup', @block_fatrix_kron_block_setup, ...
	'mtimes_block', @block_fatrix_kron_mtimes_block, ...
	'free', @block_fatrix_free);

%
% block_fatrix_kron_forw(): y = A * x
%
function y = block_fatrix_kron_forw(arg, x)

if nrow(x) ~= arg.dim(2)
	fail('x size=%d vs dim(2)=%d', nrow(x), arg.dim(2))
end
y = [];
for ii=1:arg.Mkron
	t = arg.blocks{1} * x([arg.jstart(ii):arg.jend(ii)], :);
	y = [y; t];
end

%
% block_fatrix_kron_back(): x = A' * y
% full backprojection
%
function x = block_fatrix_kron_back(arg, y)

if nrow(y) ~= arg.dim(1), error 'bad y size', end
x = [];
for ii=1:arg.Mkron
	t = arg.blocks{1}' * y([arg.istart(ii):arg.iend(ii)], :);
	x = [x; t];
end

%
% block_fatrix_kron_block_setup()
% apply Gblock to the base block, to prepare it for later
%
function ob = block_fatrix_kron_block_setup(ob, varargin)
ob.arg.blocks = {Gblock(ob.arg.blocks{1}, ob.nblock)};
% todo: probably need to set up some other internal variables here
% for use inside block_fatrix_kron_mtimes_block()

%
% block_fatrix_kron_mtimes_block(): y = A{ib} * x
%
function y = block_fatrix_kron_mtimes_block(arg, is_transpose, x, iblock, nblock)

bl1 = arg.blocks{1}; % base block, already put through Gblock
warn 'todo: size check not done'

if ~is_transpose % forw
%	if nrow(x) ~= arg.dim(2)
%		fail('x size=%d vs dim(2)=%d', nrow(x), arg.dim(2))
%	end
	y = [];
	for ii=1:arg.Mkron
		t = bl1{iblock} * x([arg.jstart(ii):arg.jend(ii)], :);
		y = [y; t];
	end

else % back
	y = x;
%	if nrow(y) ~= arg.dim(1), error 'bad y size', end
	x = [];
	for ii=1:arg.Mkron
		tmp = [arg.istart(ii):arg.iend(ii)]; % i list (if all data)
		% todo: we need a certain subset of that list
		fail('todo: kron subset backprojector not done')
		t = bl1{iblock}' * y(tmp, :);
		x = [x; t];
	end
	y = x;
end

%
% block_fatrix_sum()
%
function ob = block_fatrix_sum(blocks, arg)

dim = size(blocks{1});
for ii=1:length(blocks)
	if ~isequal(size(blocks{ii}), dim)
		error 'blocks must have same size for "sum"'
	end
end

% build Fatrix object
ob = Fatrix(dim, arg, 'caller', 'block_fatrix(sum)', ...
	'forw', @block_fatrix_sum_forw, 'back', @block_fatrix_sum_back, ...
	'free', @block_fatrix_free);

%
% block_fatrix_sum_forw(): y = A * x
%
function y = block_fatrix_sum_forw(arg, x)

y = 0;
for ii=1:length(arg.blocks)
	y = y + arg.blocks{ii} * x;
end

%
% block_fatrix_sum_back(): x = A' * y
% full backprojection
%
function x = block_fatrix_sum_back(arg, y)

x = 0;
for ii=1:length(arg.blocks)
	x = x + arg.blocks{ii}' * y;
end

%
% block_fatrix_free()
%
function block_fatrix_free(arg)
if arg.chat
	printm 'freeing block_fatrix object static memory'
end
for ii=1:length(arg.blocks)
	try
		free(blocks{ii})
	catch
	end
end

%
% block_fatrix_update()
%
function out = block_fatrix_update(ob, varargin)
% todo: figure out how to update, e.g., new_zmap(s), ...


 function [mass_atten, kev, mtype, file] = ...
		xray_read_atten(mtype, kev_in, varargin)
%function [mass_atten, kev, mtype, file] = ...
%|		xray_read_atten(mtype, kev_in, [options])
%|
%| Read mass attenuation coefficients for a given material type.
%| Optionally interpolate onto desired energies.
%|
%| in
%|	mtype			'aluminum', 'copper', 2, '2', '02-helium', ...
%|				See xray_material_file_name.m
%|		(Optionally mtype can be a cell array of several materials.
%|		If so, kev_in is mandatory and the output will be an array.)
%|	kev_in		[N 1]	optional desired energies [in keV]
%|
%| options
%|	'units'		cm | mm	default: cm
%|	'interp'	{}	interpolator type.  default {'pchip', 'extrap'}
%|	shortfile	0|1	return short file name instead of full path
%|
%| out
%|	mass_atten	[L 1]	mass attenuation coefficients [cm^2/g],
%|				If "energy" input is provided, then [N L].
%|	kev		[N 1]	at these corresponding energies.
%|	mtype		{L}	material type
%|	file		{L}	file name(s) for each material
%|
%| Copyright 2004-05-1, Jeff Fessler, University of Michigan

% default is to show example
if nargin < 1, ir_usage, end
if nargin == 1 && streq(mtype, 'test'), xray_read_atten_test, return, end
if ~isvar('kev_in'), kev_in = []; end

if isnumeric(mtype) && length(mtype) > 1
	mtype = num2cell(mtype);
end

if iscell(mtype)
	if isempty(kev_in), error 'kev_in required', end
	mass_atten = zeros(length(kev_in), length(mtype)); % array
	for ll=1:length(mtype)
		[mass_atten(:,ll) dum mtype{ll} file{ll}] = ...
			xray_read_atten(mtype{ll}, kev_in, varargin{:});
	end
	kev = kev_in;
return
end

% defaults
arg.units = 'cm';
%arg.interp = {'linear', 'extrap'};
%arg.interp = {'spline', 'extrap'};
arg.interp = {'pchip', 'extrap'};
arg.shortfile = false;

arg = vararg_pair(arg, varargin);

is_mm = 0;
if streq(arg.units, 'mm')
	is_mm = 1;
elseif ~streq(arg.units, 'cm')
	error 'bad units'
end

file = xray_material_file_name(mtype);
tmp = load_ascii_skip_header(file); % read uncommented lines
kev = tmp(:,1) * 1000;	% keV
mass_atten = tmp(:,2);	% mass atten. coeff. [cm^2/g]

if arg.shortfile
	file = regexprep(file, '.*\/', '');
end

if 0 % look at k-edges
	jump = find(diff(kev) == 0);
	if ~isempty(jump)
		file = regexprep(file, '.*\/', '');
		jump = num2str(kev(jump)', '%5.1f');
		printf('%s %s', file, jump)
	end
end

% if input energies are specified, then interpolate onto those.
% trick: allow for the k-edge jumps!
if isvar('kev_in') && ~isempty(kev_in)
	mass_atten = log(mass_atten); % interpolate on a log scale
	mass_atten = interp1_jump(kev, mass_atten, kev_in, ...
		'monodown', arg.interp{:});
	mass_atten = exp(mass_atten);
	kev = kev_in;
end

if is_mm
	mass_atten = mass_atten * 100;
end

% xray_read_atten_test()
% example usage
function xray_read_atten_test
mtypes = {'lead', 'aluminum', 'water', 'lexan'};
mtypes = {'water', 'bone'};
mtypes = {'iodine', 'cesium', 'csi'};
arg1 = {};
kev = [10:510]';
for ll=1:length(mtypes)
	mtype = mtypes{ll};
	[mac1 kev1] = xray_read_atten(mtype);
	ie = min(kev) <= kev1 & kev1 <= max(kev);
	arg1 = {arg1{:}, kev1(ie), mac1(ie), 'o'};
end
mac2 = xray_read_atten(mtypes, kev); % test multiple types (cell)
if im
	clf, semilogy(arg1{:})
	legend(mtypes{:})
	hold on, semilogy(kev, mac2, '-'), hold off
	xlabel 'KeV', ylabel 'mass atten. coef.', axis tight
end


function [EToE,EToF]= tiConnect2D(EToV)

% function [EToE,EToF]= tiConnect2D(EToV)
% Purpose: triangle face connect algorithm due to Toby Isaac

Nfaces=3;
K = size(EToV,1);
Nnodes = max(max(EToV));

% create list of all faces 1, then 2, & 3
fnodes = [EToV(:,[1,2]);EToV(:,[2,3]);EToV(:,[3,1])];
fnodes = sort(fnodes,2)-1;

% set up default element to element and Element to faces connectivity
EToE= (1:K)'*ones(1,Nfaces); EToF= ones(K,1)*(1:Nfaces);

% uniquely number each set of three faces by their node numbers
id = fnodes(:,1)*Nnodes + fnodes(:,2)+1;
spNodeToNode=[id, (1:Nfaces*K)', EToE(:), EToF(:)];

% Now we sort by global face number.
sorted=sortrows(spNodeToNode,1);

% find matches in the sorted face list
[indices,dummy]=find( sorted(1:(end-1),1)==sorted(2:end,1) );

% make links reflexive
matchL = [sorted(indices,:)   ;sorted(indices+1,:)];
matchR = [sorted(indices+1,:) ;sorted(indices,:)];

% insert matches
EToE(matchL(:,2)) = matchR(:,3); EToF(matchL(:,2)) = matchR(:,4);
return;


function [parameters, stderrors, LLF, ht, resids, summary] = garchfind(data, models, distributions, ar, ma, p, q, options)
%{
[parameters, stderrors, LLF, ht, resids, summary] = garch(data, model, distr, ar, ma, x, p, q, y,startingvalues, options)
-----------------------------------------------------------------------
 PURPOSE:
 Finds the combination of models and distributions that better fits the
 data based on a set of criteria (i.e.  largest log likelihood value
 and the smallest AIC and BIC criteria).
----------------------------------------------------------------------
 USAGE:
 garchfind(data, models, distributions, ar, ma, p, q)

 INPUTS:
 data:          (T x 1) vector of data
 models:        a character vector with models to be estimated
 distributions: a character vector with distributions
 ar:            length of AR
 ma:            length of MA
 p:             length of ARCH
 q:             length of GARCH
 options:       set of options

 OUTPUTS based on the estimation of the best model:
 parameters:   a vector of parameters a0, a1, b0, b1, b2, ...
 stderrors:    standard errors estimated by the inverse Hessian (fmincon)
 LFF:          the value of the Log-Likelihood Function
 ht:           vector of conditional variances
 resids:       vector of residuals
 summary:      summary of results including:
                -model specification, distribution and statistics
                -optimization options
                -t-statistics
                -robust standard errors: (HESSIAN^-1)*cov(scores)*(HESSIAN^-1)
                -scores: numerical scores for M testing
-----------------------------------------------------------------------
Author:
 Alexandros Gabrielsen, a.gabrielsen@city.ac.uk
 Date: 08/2011
-----------------------------------------------------------------------
%}
if nargin == 0
    error('Data, GARCH Models, Distribution, AR, MA, ARCH, GARCH, Options')
end

if size(data,2) > 1
   error('Data vector should be a column vector')
end

if (length(ar) > 1) | (length(ma) > 1) | ar < 0 | ma < 0
    error('AR and MA should be positive scalars')
end

if (length(p) > 1) | (length(q) > 1) | p < 0 | q < 0
    error('P and Q should be positive scalars')
end

Criteria=[];
index=1;
for i = 1:size(models,1)
    for j = 1:size(distributions,1)
        for a=1:ar
            for b=1:ma
                for c=1:p
                    for d=1:q
                         fprintf('Progress: ARMA(%1.0f,%1.0f,%1.0f)-%s(%1.0f,%1.0f,%1.0f) - %s distribution\n', a, b, 0, strcat(models(i,:)), c, d, 0, strcat(distributions(j,:)))
                         eval(['[parameters, stderrors, LLF_temp] = garch(data,''' strcat(models(i,:)),''',''', strcat(distributions(j,:)), ''',a,b,0,c,d,0,[],options);']);
                         Criteria(index,:) = [LLF_temp, -2*LLF_temp+2*size(parameters,1), -2*LLF_temp+size(parameters,1)*log(size(data,1)), i, j, a, b, c,d];
                         index=index+1;
                    end
                end
            end
        end
    end
end

% This estimates a vector of results as long as the criteria are satisfied
holder=sortrows(Criteria,1);
z=size(holder,1)+1;
fprintf('\n The top models are: \n')
for i = 1:size(holder,1)
    if  find(holder == max(holder(1:z-i,1))) == find(holder == min(holder(1:z-i,2))) - size(holder(1:z-i,1),1) & find(holder == max(holder(1:z-i,1))) == find(holder == min(holder(1:z-i,3))) - 2*size(holder(1:z-i,1),1)
        fprintf('ARMA(%1.0f,%1.0f,%1.0f)-%s(%1.0f,%1.0f,%1.0f) - %s distribution\n', holder(z-i,6), holder(z-i,7), 0, strcat(models(holder(z-i,4),:)), holder(z-i,8), holder(z-i,9), 0, strcat(distributions(holder(z-i,5),:)))
    end
end

% Finds the best model so as to estimate the model
if find(Criteria == max(Criteria(:,1))) == find(Criteria == min(Criteria(:,2))) - size(Criteria,1) & find(Criteria == max(Criteria(:,1))) == find(Criteria == min(Criteria(:,3))) - 2*size(Criteria,1)
[parameters, stderrors, LLF, ht, resids, summary] = garch(data, strcat(models(Criteria(find(Criteria == max(Criteria(:,1))),4),:)), ...
    strcat(distributions(Criteria(find(Criteria == max(Criteria(:,1))),5),:)), ...
    Criteria(find(Criteria == max(Criteria(:,1))),6),...
    Criteria(find(Criteria == max(Criteria(:,1))),7),0, ...
    Criteria(find(Criteria == max(Criteria(:,1))),8), ...
    Criteria(find(Criteria == max(Criteria(:,1))),9),0,[],options);
end
end


function tetra_node2 = tet_mesh_order10_to_order4_compute ( tetra_num1, ...
  tetra_node1, tetra_num2 )

%*****************************************************************************80
%
%% TET_MESH_ORDER10_TO_ORDER4_COMPUTE linearizes a quadratic tet mesh.
%
%  Discussion:
%
%    A quadratic tet mesh is assumed to consist of 10-node
%    tetrahedrons.
%
%    This routine rearranges the information so as to define a 4-node
%    tet mesh.
%
%    The same nodes are used, but there are 8 times as many
%    tetrahedrons.
%
%    The node ordering for the quadratic tetrahedron is somewhat
%    arbitrary.  In the current scheme, the vertices are listed
%    first, followed by the 6 midside nodes.  Each midside node
%    may be identified by the two vertices that bracket it.  Thus,
%    the node ordering may be suggested by:
%
%      1  2  3  4 (1+2) (1+3) (1+4) (2+3) (2+4) (3+4)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Anwei Liu, Barry Joe,
%    Quality Local Refinement of Tetrahedral Meshes Based
%    on 8-Subtetrahedron Subdivision,
%    Mathematics of Computation,
%    Volume 65, Number 215, July 1996, pages 1183-1200.
%
%  Parameters:
%
%    Input, integer TETRA_NUM1, the number of tetrahedrons in the quadratic
%    tet mesh.
%
%    Input, integer TETRA_NODE1(10,TETRA_NUM1), the quadratic tet mesh.
%
%    Input, integer TETRA_NUM2, the number of tetrahedrons in the linear
%    tet mesh.  TETRA_NUM2 = 8 * TETRA_NUM1.
%
%    Output, integer TETRA_NODE2(4,TETRA_NUM2), the linear tet mesh.
%
  tetra2 = 0;

  for tetra1 = 1 : tetra_num1

    n1 = tetra_node1(1,tetra1);
    n2 = tetra_node1(2,tetra1);
    n3 = tetra_node1(3,tetra1);
    n4 = tetra_node1(4,tetra1);
    n5 = tetra_node1(5,tetra1);
    n6 = tetra_node1(6,tetra1);
    n7 = tetra_node1(7,tetra1);
    n8 = tetra_node1(8,tetra1);
    n9 = tetra_node1(9,tetra1);
    nx = tetra_node1(10,tetra1);

    tetra2 = tetra2 + 1;
    tetra_node2(1:4,tetra2 ) = [ n1, n5, n6, n7 ]';
    tetra2 = tetra2 + 1;
    tetra_node2(1:4,tetra2 ) = [ n2, n5, n8, n9 ]';
    tetra2 = tetra2 + 1;
    tetra_node2(1:4,tetra2 ) = [ n3, n6, n8, n9 ]';
    tetra2 = tetra2 + 1;
    tetra_node2(1:4,tetra2 ) = [ n4, n7, n9, nx ]';
    tetra2 = tetra2 + 1;
    tetra_node2(1:4,tetra2)  = [ n5, n6, n7, n9 ]';
    tetra2 = tetra2 + 1;
    tetra_node2(1:4,tetra2 ) = [ n5, n6, n8, n9 ]';
    tetra2 = tetra2 + 1;
    tetra_node2(1:4,tetra2 ) = [ n6, n7, n9, nx ]';
    tetra2 = tetra2 + 1;
    tetra_node2(1:4,tetra2 ) = [ n6, n8, n9, nx ]';

  end

  return
end


function result = cvRsimpls(x,y,kmax,rmsecv,h,k)

%CVRIMPLS calculates the robust RMSECV (root mean squared error of cross-validation) curve
% for RSIMPLS or the robust RMSEP(root mean squared error of prediction) value in a fast way.
% The R-RMSECV curve can be used to make a selection of the optimal number of
% components to include in the regression model. The function is used in rsimpls.m.
%
% Input arguments:
%    x          : the explanatory variables
%    y          : the rsponse variables
%    kmax       : maximal number of variables to include in the model.
%    rmsecv     : Optional. If equal to 1 (default), the rmsecv is computed.
%                 Else, rmsecv = 0 and then the rss and R2 are computed.
%    h          : optional input argument.
%    k          : optional input argument. If k = 0 (default) then RMSECV is calculated. Else the RMSEP wil be computed.
%
% Output:
%  if RMSECV is computed:
%   result.rmsecv     : the R-RMSECV values (obtained with the minimum weights).
%  if RMSEP is computed:
%   result.rmsep      : the R-RMSEP values
%   result.rss        : the RSS values for every k.
%   result.R2         : the coefficient of determination for every k.
%   result.residu     : the residuals for every k = 1,...,kmax
%   result.outWeights : the weights used to compute the robust R-RMSEP values.
%
% This function is part of LIBRA: the Matlab Library for Robust Analysis,
% available at:
%              http://wis.kuleuven.be/stat/robust.html
%
% Written by Sanne Engelen
% Last Update: 08/10/2004, 03/07/2006
% Last Revision: 03/07/2006

% some initialisations
n = size(x,1);
p = size(x,2);
q = size(y,2);
r = rank(x);
rz = rank([x,y]);
teller_if_lus = 0;
cutoffWeights = sqrt(chi2inv(0.975,q));

if nargin < 4
    alfa = 0.75;
    kmaxr=min([kmax+q,rz]);
    h=floor(2*floor((n+kmaxr+1)/2)-n+2*(n-floor((n+kmaxr+1)/2))*alfa);
    k = 0;
    rmsecv = 1;
elseif nargin == 4
    alfa = 0.75;
    kmaxr=min([kmax+q,rz]);
    h=floor(2*floor((n+kmaxr+1)/2)-n+2*(n-floor((n+kmaxr+1)/2))*alfa);
    k = 0;
elseif nargin == 5
    k = 0;
end

outWeights = weightcvRsimpls(x,y,kmax,h,k,cutoffWeights);

if rmsecv
    if k == 0
        w_min = outWeights.w_min;
    end
    rob = outWeights.ResRob;

    % Assigning the input variables
    if size(rob.Hsubsets.H0,2)==1
        rob.Hsubsets.H0=rob.Hsubsets.H0';
    end
    Hsets = [rob.Hsubsets.H0;rob.Hsubsets.H1;rob.Hsubsets.Hfreq];
    same.value = 0;
    data = [x,y];

    for i = 1:n
        disp(['observation ',num2str(i),' is left out'])
        X_min_i = removal(x,i,0);
        Y_min_i = removal(y,i,0);
        data_min_i = removal(data,i,0);

        same.value = 0;
        if isempty(find(rob.Hsubsets.H0 == i))
            if teller_if_lus >= 1
                same.value = 1;
            end
            teller_if_lus = teller_if_lus + 1;
        end

        % constructing Hsets of right size: h - 1.
        Hsets_min_i = RemoveObsHsets(Hsets,i);

        if k == 0
            res = removeObsRobpca(data,i,kmax + q,Hsets_min_i,same,1);
        else
            res = removeObsRobpca(data,i,k + q,Hsets_min_i,same);
        end

        if isempty(find(rob.Hsubsets.H0 == i))
            same.res = res;
        end

        Prob_min_i = res.Pk_min_i;
        Lrob_min_i = res.Lk_min_i;
        murob_min_i = res.muk_min_i;
        Trob_min_i = (data_min_i - repmat(murob_min_i,n-1,1))*Prob_min_i;

        % Computing weights corresponding with the ROBPCA results.
        sdrob_min_i = sqrt(libra_mahalanobis(Trob_min_i,zeros(1,size(Trob_min_i,2)),'invcov',1./Lrob_min_i))';

        if k == 0
            cutoff.sd=sqrt(chi2inv(0.975,kmax));
        else
            cutoff.sd = sqrt(chi2inv(0.975,k));
        end

        % Orthogonal distances to robust PCA subspace
        XRc=data_min_i-repmat(murob_min_i,n-1,1);
        Xtilde=Trob_min_i*Prob_min_i';
        Rdiff=XRc-Xtilde;
        for j=1:(n-1)
            odrob_min_i(j,:)=norm(Rdiff(j,:));
        end
        % Robust cutoff-value for the orthogonal distance
        if k == 0
            test_k = kmax;
        else
            test_k = k;
        end

        if test_k~=r
            [m,s]=unimcd(odrob_min_i.^(2/3),h);
            cutoff.od = sqrt(norminv(0.975,m,s).^3);
            wrob_min_i = (odrob_min_i<=cutoff.od)&(sdrob_min_i<=cutoff.sd);
        else
            cutoff.od=0;
            wrob_min_i = (sdrob_min_i<=cutoff.sd);
        end

        % start the deflation:
        xycentr_min_i = [];
        sigmax_min_i = [];
        xcentr_min_i = [];
        sigmaxy_min_i = [];
        sigmayx_min_i = [];

        xycentr_min_i = murob_min_i;
        sigmax_min_i = Prob_min_i(1:p,:)*diag(Lrob_min_i)*Prob_min_i(1:p,:)';
        xcentr_min_i = X_min_i - repmat(murob_min_i(1:p),n-1,1);
        sigmaxy_min_i = Prob_min_i(1:p,:)*diag(Lrob_min_i)*Prob_min_i(p+1:p+q,:)';
        sigmayx_min_i = sigmaxy_min_i';

        % calculation of the scores.
        nScores = 1;
        R_min_i = [];
        T_min_i = [];
        P_min_i = [];
        V_min_i = [];

        if k == 0
            countScores = kmax;
        else
            countScores = k;
        end

        while nScores <= countScores
            if q == 1
                qq_min_i = 1;
            else
                [QQ,LL] = eig(sigmayx_min_i*sigmaxy_min_i);
                [LL,I] = greatsort(diag(LL));
                qq_min_i = QQ(:,I(1));
            end

            rr_min_i = sigmaxy_min_i*qq_min_i;
            rr_min_i = rr_min_i/norm(rr_min_i);
            tt_min_i = xcentr_min_i*rr_min_i;
            pp_min_i = sigmax_min_i*rr_min_i/(rr_min_i'*sigmax_min_i*rr_min_i);
            vv_min_i = pp_min_i;

            if nScores > 1
                vv_min_i = vv_min_i - V_min_i*(V_min_i'*pp_min_i);
            end
            vv_min_i = vv_min_i./norm(vv_min_i);
            sigmaxy_min_i = sigmaxy_min_i - vv_min_i*(vv_min_i'*sigmaxy_min_i);

            V_min_i(:,nScores) = vv_min_i;
            T_min_i(:,nScores) = tt_min_i;
            R_min_i(:,nScores) = rr_min_i;
            P_min_i(:,nScores) = pp_min_i;

            nScores = nScores + 1;
        end

        if k == 0
            outRegr = runRegr(x,y,i,T_min_i,Y_min_i,R_min_i,murob_min_i,wrob_min_i,kmax,cutoffWeights);
            for j = 1:kmax
                Tk_min_i = T_min_i(:,1:j);
                geg = [Tk_min_i,Y_min_i];

                outRegr.Mu = outRegr.center';
                outRegr.Sigma = outRegr.sigma;
                [Bk,intk,sigmayykmaxrew_k,sigmattkmaxrew_k] = extractmcdregres(outRegr,Tk_min_i,Y_min_i,kmax,n-1,q,j,h-1,cutoffWeights);
                coeffk = [Bk;intk];
                b_min_i = R_min_i(:,1:j)*coeffk(1:j,:);
                int_min_i = coeffk(j+1,:)  - murob_min_i(1:p)*R_min_i(:,1:j)*coeffk(1:j,:);
                Yhat_min_i = x(i,:)*b_min_i + int_min_i;
                resid_min_i(i,(j-1)*q + 1:j*q) = y(i,:) - Yhat_min_i;

                % calculation of the resd:
                rewE2=sigmayykmaxrew_k- coeffk(1:j,1:q)'*sigmattkmaxrew_k*coeffk(1:j,1:q);

                if q > 1
                    cov = rewE2;
                    cen=zeros(q,1);
                    resd(i,j)=sqrt(libra_mahalanobis(resid_min_i(i,(j-1)*q + 1:j*q),cen','cov',cov))'; %robust distances of residuals
                else
                    scale = sqrt(rewE2);
                    resd(i,j) = resid_min_i(i,(j-1)*q + 1:j*q)/scale;
                end
            end
        else
            outRegr = runRegr(x,y,i,T_min_i,Y_min_i,R_min_i,murob_min_i,wrob_min_i,k,cutoffWeights);
            resid_min_i(i,:) = outRegr.resid_min_i;
            resd(i,:) = outRegr.resd';
        end
    end

    if k == 0
        for j = 1:kmax
            resk = resid_min_i(:,(j-1)*q + 1:j*q);
            if q == 1
                rmsecv(j) = sqrt(1/sum(w_min)*w_min*(resk).^2);
            else
                rmsecv(j) = sqrt(1/sum(w_min)*w_min*(mean((resk').^2))');
            end
        end
        result.rmsecv = rmsecv;
        result.residu = resid_min_i;
    else
        weights = outWeights.weightsk;
        if q == 1
            rmsep = sqrt(1/sum(weights)*weights'*(resid_min_i).^2);
        else
            rmsep = sqrt(1/sum(weights)*weights'*(mean((resid_min_i').^2))');
        end
        result.rmsep = rmsep;
        result.residu = resid_min_i;
    end
end

result.outWeights = outWeights;
result.rss = outWeights.rss;
result.R2 = outWeights.R2;

%------------------------------------------------------------------
function out = runRegr(x,y,i,T_min_i,Y_min_i,R_min_i,mukmax_min_i,wkmax_min_i,k,cutoffWeights)

[n,p] = size(x);
[n,q] = size(y);

% perform the robpca regression:
breg = [];
b_min_i = [];
int_min_i= [];
Yhat_min_i = [];
robpcareg = robpcaregres(T_min_i(:,1:k),Y_min_i,wkmax_min_i',cutoffWeights);
out.center = robpcareg.center;
out.sigma = robpcareg.sigma;
breg = robpcareg.coeffs(1:k,:);
b_min_i = R_min_i(:,1:k)*breg;
int_min_i = robpcareg.coeffs(k+1,:) - mukmax_min_i(1:p)*R_min_i(:,1:k)*breg;
Yhat_min_i = x(i,:)*b_min_i + int_min_i;
resid_min_i = y(i,:) - Yhat_min_i;

% calculation of the resd:
if q > 1
    cov = robpcareg.cov;
    cen=zeros(q,1);
    resd=sqrt(libra_mahalanobis(resid_min_i,cen','cov',cov))'; %robust distances of residuals
else
    scale = sqrt(robpcareg.cov);
    resd = resid_min_i/scale;
end

out.resid_min_i = resid_min_i;
out.resd = resd;
%----------------------------------------------------------------------------------------
function out = weightcvRsimpls(x,y,kmax,h,k,cutoffWeights)

% Computes the weights for the robust RMSECV/RMSEP values.
%
% input:
%   x    : the independent variables.
%   y    : the response variables.
%   kmax : the maximal number of components to be considered.
%   h    : the number of observations on which the calculations are based.
%   k    : if equal to zero, robpca is performed on kmax components (case RMSECV). (default).
%          Else, robpca is performed for a certain number of components (case RMSEP)
%
% output:
%   out.w_min    : the weights obtained by taking the minimum over all k
%   out.weightsk : the weights for all observations and all k (n x kmax)
%   out.resrob   : the results of robpca on [x,y].
%   out.R2       : the weighted Rsquared for each value of k
%   out.rss      : the weighted rss for each value of k

n = size(x,1);
p = size(x,2);
q = size(y,2);
r = rank(x);

if nargin < 5
    k = 0;
end

if k == 0
    ResRob = robpca([x,y],'plots',0,'k',kmax + q,'kmax',kmax + q,'h',h);
else
    ResRob = robpca([x,y],'plots',0,'k',k + q,'kmax',kmax + q,'h',h);
end

Trob = ResRob.T;
Prob = ResRob.P;
Lrob = ResRob.L;
murob = ResRob.M;
wrob = ResRob.flag.all;

%deflation

xycentr = [];
sigmax = [];
xcentr = [];
sigmaxy = [];
sigmayx = [];

xycentr = murob;
sigmax = Prob(1:p,:)*diag(Lrob)*Prob(1:p,:)';
xcentr = x - repmat(murob(1:p),n,1);
sigmaxy = Prob(1:p,:)*diag(Lrob)*Prob(p+1:p+q,:)';
sigmayx = sigmaxy';

% calculation of the scores.
nScores = 1;
R = [];
T = [];
P = [];
V = [];

if k == 0
    countScores = kmax;
else
    countScores = k;
end

while nScores <= countScores
    if q == 1
        qq = 1;
    else
        [QQ,LL] = eig(sigmayx*sigmaxy);
        [LL,I] = greatsort(diag(LL));
        qq = QQ(:,I(1));
    end

    rr = sigmaxy*qq;
    rr = rr/norm(rr);
    tt = xcentr*rr;
    pp = sigmax*rr/(rr'*sigmax*rr);
    vv = pp;

    if nScores > 1
        vv = vv - V*(V'*pp);
    end
    vv = vv./norm(vv);
    sigmaxy = sigmaxy - vv*(vv'*sigmaxy);

    V(:,nScores) = vv;
    T(:,nScores) = tt;
    R(:,nScores) = rr;
    P(:,nScores) = pp;

    nScores = nScores + 1;
end

if k == 0
    outRobRegr = robpcaregres(T(:,1:kmax),y,wrob');

    for j = 1:kmax
        outRobRegr.Mu = outRobRegr.center';
        outRobRegr.Sigma = outRobRegr.sigma;
        [Bk,intk,sigmayykmaxrew_k,sigmattkmaxrew_k] = extractmcdregres(outRobRegr,T(:,1:j),y,kmax,n,q,j,h,cutoffWeights);
        coeffk = [Bk;intk];
        finalB = R(:,1:j)*coeffk(1:j,:);
        finalInt = coeffk(j+1,:)  - murob(1:p)*R(:,1:j)*coeffk(1:j,:);
        Yhat = x*finalB + repmat(finalInt,n,1);
        resid(:,(j-1)*q+1:j*q) = y - Yhat;

        % calculation of the rd:
        rewE2=sigmayykmaxrew_k- coeffk(1:j,1:q)'*sigmattkmaxrew_k*coeffk(1:j,1:q);

        if q > 1
            cov = rewE2;
            cen=zeros(q,1);
            resd = sqrt(libra_mahalanobis(resid(:,(j-1)*q+1:j*q),cen','cov',cov))'; %robust distances of residuals
            weightsk(:,j) = (abs(resd)<=cutoffWeights);
        else
            scale = sqrt(rewE2);
            resd = resid(:,(j-1)*q+1:j*q)/scale;
            weightsk(:,j) = (abs(resd)<=cutoffWeights);
        end
    end
else
    outRobRegr = robpcaregres(T(:,1:k),y,wrob');

    % robust residual distance:
    if q==1
        resd=outRobRegr.resids/sqrt(outRobRegr.cov);
    else
        resd=sqrt(libra_mahalanobis(outRobRegr.resids,zeros(1,q),'cov',outRobRegr.cov))';
    end

    weightsk = (abs(resd)<=cutoffWeights);
end

if k == 0
    if kmax == 1
        w_min = weightsk';
    else
        w_min = min(weightsk');
    end

    out.w_min = w_min;
    out.weightsk = weightsk;

    yw = mean(y(w_min == 1,:));
    y2=(y-repmat(yw,n,1)).^2;
    R = resid.^2;
    D=sum(y2(w_min==1,:));
    for j = 1:kmax
        R1=R(w_min==1,(j-1)*q+1:j*q);
        rss(j) = sum(sum(R1));
        R2(j)=1-rss(j)/sum(D);
    end

    out.rss = 1/(q*sum(w_min))*rss;
    out.R2 = R2;
else
    out.weightsk = weightsk;

    s=sum(weightsk);
    yw=sum(y(weightsk==1,:))/s;
    y2=(y-repmat(yw,n,1)).^2;
    R = outRobRegr.resids.^2;
    D=sum(y2(weightsk==1,:));
    R1=R(weightsk==1,:);
    rss = sum(sum(R1));
    R2=1-rss/sum(D);

    out.rss = 1/(q*sum(weightsk))*rss;
    out.R2 = R2;
end
out.ResRob = ResRob;
%---------------------------------------------------------------------------------------
function Hsets_min_i = RemoveObsHsets(Hsets,i)

% removes the right index from the $h$-subsets in Hsets to
% obtain (h - 1)-subsets.
% every h-set is put as a row in Hsets.
% i is the index of the observation that is removed from the whole data.

for r = 1:size(Hsets,1)
    if ~isempty(find(Hsets(r,:)== i))
        Hsets_min_i(r,:) = removal(Hsets(r,:),0,find(Hsets(r,:) == i));
    else
        Hsets_min_i(r,:) = Hsets(r,1:(end-1));
    end

    for j = 1:length(Hsets_min_i(r,:))
        if Hsets_min_i(r,j) > i
            Hsets_min_i(r,j) = Hsets_min_i(r,j) - 1;
        end
    end
end


function [z,flag] = formationP2z(polytope,zd,range,lc,lm)
max_aw = 1;
min_aw = 0.45;
polynum = length(polytope);
% get the A and b from polytope
Ap = []; bp = [];
for i=1:polynum
    Ap = [Ap;polytope(i).A];
    bp = [bp;polytope(i).b];
end
if length(zd)==12
    % constraints al<=a<=au and thl<=th<=thu just use lb and ub
    lb = [range.lb;-pi;min_aw*ones(4,1);-pi/4*ones(4,1);min_aw]';
    ub = [range.ub;pi;max_aw*ones(4,1);pi/4*ones(4,1);max_aw]';
    % calculate z
    [z,~,flag] = fmincon(@(z) (z-zd)*(z-zd)',zd,[],[],[],[],lb,ub,@(z) mycon(z,Ap,bp,lc,lm));
else
    % constraints al<=a<=au and thl<=th<=thu just use lb and ub
    lb = [range.lb;min_aw*ones(2,1)]';
    ub = [range.ub;max_aw*ones(2,1)]';
    % calculate z
    [z,~,flag] = fmincon(@(z) (z-zd)*(z-zd)',zd,[],[],[],[],lb,ub,@(z) mycon2(z,Ap,bp,lc,lm));
end
end

function [c,ceq] = mycon(z,A,b,lc,lm)
import rvctools.*
max_aw = 1;
min_aw = 0.45;
xr = [z(1:2),0,0,0,z(3)]; a = z(4:7); th = z(8:11); w = z(12);
xm = xr2m(xr,a,th,lc,lm);
for i=1:size(xm,1)
    plat(i) = pkgMechanics.RigidCuboid(1,xm(i,:),lm);
    pm_v(:,:,i) = plat(i).vertices(1:4,:)';
    Rm(:,:,i) = rpy2r(xm(i,4:end));
    pw_m(:,i) = xm(i,1:3)';
    pw_v(:,4*i-3:4*i) = pw_m(:,i)+Rm(:,:,i)*pm_v(:,:,i);
end
for i=1:size(pw_v,2)
    c1(:,i) = A*pw_v(1:2,i)-b;
end
c2 = (a.^2+w.^2-max_aw^2)';
c3 = -(a.^2+w.^2-min_aw^2)';
c = [c1(:);c2(:);c3(:)];
ceq = 0;
end

function [c,ceq] = mycon2(z,A,b,lc,lm)
import rvctools.*
max_aw = 1;
min_aw = 0.45;
xr = [z(1:2),0,0,0,0]; a = z(3)*ones(1,4); th = zeros(1,4); w = z(4);
xm = xr2m(xr,a,th,lc,lm);
for i=1:size(xm,1)
    plat(i) = pkgMechanics.RigidCuboid(1,xm(i,:),lm);
    pm_v(:,:,i) = plat(i).vertices(1:4,:)';
    Rm(:,:,i) = rpy2r(xm(i,4:end));
    pw_m(:,i) = xm(i,1:3)';
    pw_v(:,4*i-3:4*i) = pw_m(:,i)+Rm(:,:,i)*pm_v(:,:,i);
end
for i=1:size(pw_v,2)
    c1(:,i) = A*pw_v(1:2,i)-b;
end
c2 = (a.^2+w.^2-max_aw^2)';
c3 = -(a.^2+w.^2-min_aw^2)';
c = [c1(:);c2(:);c3(:)];
ceq = 0;
end


function [sol,info] = solvebilevel(OuterConstraints,OuterObjective,InnerConstraints,InnerObjective,InnerVariables,options)
%SOLVEBILEVEL Simple global bilevel solver
%
%   min        OO(x,y)
%   subject to CO(x,y)>=0
%              y = arg min OI(x,y)
%              subject to CI(x,y)>=0
%
%   [DIAGNOSTIC,INFO] = SOLVEBILEVEL(CO, OO, CI, OI, y, options)
%
%   diagnostic : Struct with standard YALMIP diagnostics
%   info       : Bilevel solver specific information
%
%   Input
%      CO       : Outer constraints (linear elementwise)
%      OO       : Outer objective (convex quadratic)
%      CI       : Inner constraints (linear elementwise)
%      OI       : Inner objective (convex quadratic)
%      y        : Inner variables
%      options  : solver options from SDPSETTINGS.
%
%   The behaviour of the bilevel solver can be controlled
%   using the field 'bilevel' in SDPSETTINGS
%
%      bilevel.outersolver : Solver for outer problems with inner KKT removed
%      bilevel.innersolver : Solver for inner problem
%      bilevel.rootcut     : Number of cuts (based on complementary
%                            constraints) added in root (experimental)
%      bilevel.relgaptol   : Termination tolerance
%      bilevel.compslacktol: Tolerance for accepting complementary slackness
%      bilevel.feastol     : Tolerance for feasibility in outer problem
%
%
%   See also SDPVAR, SDPSETTINGS, SOLVESDP

% min f(x,y) s.t g(x,y)<0, y = argmin  [x;y]'*H*[x;y]+e'[x;y]+f, E[x;y]<d

if nargin<6
    options = sdpsettings;
elseif isempty(options)
    options = sdpsettings;
end

y = InnerVariables;

if ~isempty(InnerConstraints)
    if any(is(InnerConstraints,'sos2'))
        error('SOS2 structures not allowed in inner problem');
    end
end

% User wants to use fmincon, cplex or something like
if strcmp(options.bilevel.algorithm,'external')
    % Derive KKT conditions of inner problem, append with outer, and solve
    % using standard solver
    z = [depends(OuterConstraints) depends(OuterObjective) depends(InnerObjective) depends(InnerConstraints)];
    z = setdiff(z,depends(y));
    z = recover(unique(z));
    [K,details] = kkt(InnerConstraints,InnerObjective,z,options);
    Constraints = [K,OuterConstraints];
    options.solver = options.bilevel.outersolver;
    sol = solvesdp(Constraints,OuterObjective,options);
    info = [];
    return
end

% Export the inner model, and select solver
options.solver = options.bilevel.innersolver;
if isa(InnerObjective, 'double') || is(InnerObjective,'linear')
    [Imodel,Iax1,Iax2,inner_p] = export(InnerConstraints,InnerObjective,options,[],[],0);
elseif is(InnerObjective,'quadratic')
    % We have to be a bit careful about cases such as x'y. This is convex in
    % the inner problem, since x is constant there.
    % [Q,c,f,dummy,nonquadratic] = vecquaddecomp(InnerObjective);
    % Extract model for a fake quadratic model
    % [InnerConstraints,failure] = expandmodel(InnerConstraints,InnerObjective)
    %[Imodel,Iax1,Iax2,inner_p] = export(InnerConstraints,dummy'*diag(1+diag(Q{1}))*dummy+c{1}'*dummy,options,[],[],0);
   % toptions = options;
   % toptions.expandbilinear = 1;
    yy = recover(setdiff(depends(y),setdiff(depends(InnerObjective),depends(y))));
    [Imodel,Iax1,Iax2,inner_p] = export(InnerConstraints,yy'*yy+sum(recover(depends(InnerObjective))),options,[],[],0);
    [Q,c,f,dummy,nonquadratic] = vecquaddecomp(InnerObjective,recover(inner_p.used_variables));
    %[Imodel,Iax1,Iax2,inner_p] = export(InnerConstraints,InnerObjective,options,[],[],0);
    % Now plug in the real quadratic function
    if ~isequal(getvariables(dummy),inner_p.used_variables)
        error('This quadratic form is not supported yet. Please make feature request')
    else
        inner_p.Q = Q{1};
        inner_p.c = c{1};
        inner_p.f = f{1};
    end
else
    error('Only LPs or convex QPs allowed as inner problem');
end

% Modeling of inner problem might have lead to more decision variables in
% the inner problem. Append these
v1 = getvariables(y);
v2 = inner_p.used_variables(inner_p.extended_variables);
v3 = inner_p.used_variables(inner_p.aux_variables(:));
y = recover(unique([v1(:);v2(:);v3(:)]));

% Export the outer model, and select solver
options.solver = options.bilevel.outersolver;
options.bmibnb.diagonalize = 0;
[Omodel,Oax1,Oax2,outer_p] = export(OuterConstraints,OuterObjective,options,[],[],0);
if isstruct(Oax2)
   sol = Oax2;
   info = 2;
   return
end
% Export a joint model with KKT removed, to simplify some setup later
% [Auxmodel,Auxax1,Auxax2,outerinner_p] = export([OuterConstraints,InnerConstraints],OuterObjective+pi*InnerObjective,options,[],[],0);

if ~all(inner_p.variabletype==0) | ~isequal(inner_p.K.s,0) | ~isequal(inner_p.K.q,0)
    error('Only LPs or convex QPs allowed as inner problem');
end

if options.bilevel.rootcuts & (~(isequal(outer_p.K.s,0) & isequal(outer_p.K.q,0)))
    disp('Disjunctive cuts currently only supported when inner is a QP')
    options.bilevel.rootcuts = 0;
end

FRP0 = inner_p;

[merged_mt,merged_vt] = mergemonoms(inner_p,outer_p);

if ~isequal(inner_p.used_variables,outer_p.used_variables)
    invar  = inner_p.used_variables;
    outvar = outer_p.used_variables;

    binary_variables = unique([inner_p.used_variables(inner_p.binary_variables) outer_p.used_variables(outer_p.binary_variables)]);
    integer_variables = unique([inner_p.used_variables(inner_p.integer_variables) outer_p.used_variables(outer_p.integer_variables)]);
    semi_variables = unique([inner_p.used_variables(inner_p.semicont_variables) outer_p.used_variables(outer_p.semicont_variables)]);
    all_variables = unique([inner_p.used_variables outer_p.used_variables]);
    if ~isequal(all_variables,inner_p.used_variables )
        inner_p = pad(inner_p,all_variables);
        FRP0 = inner_p;
        FRP0.monomtable = speye(length(inner_p.c));
    end
    if ~isequal(all_variables,outer_p.used_variables )
        outer_p = pad(outer_p,all_variables);
    end
else
    binary_variables = unique([inner_p.used_variables(inner_p.binary_variables) outer_p.used_variables(outer_p.binary_variables)]);
    integer_variables = unique([inner_p.used_variables(inner_p.integer_variables) outer_p.used_variables(outer_p.integer_variables)]);
    semi_variables = unique([inner_p.used_variables(inner_p.semicont_variables) outer_p.used_variables(outer_p.semicont_variables)]);
    all_variables = inner_p.used_variables;
end

outer_p.monomtable   = merged_mt;
outer_p.variabletype = merged_vt;
inner_p.variabletype = merged_vt;
inner_p.monomtable   = merged_vt;
% Index to inner variables
for i = 1:length(y)
    y_var(i) = find(all_variables == getvariables(y(i)));
end

% Index to outer variables
x_var = setdiff(1:length(all_variables),y_var);

% Index to binary variables
bin_var = [];
for i = 1:length(binary_variables)
    bin_var(i) = find(all_variables == binary_variables(i));
end
int_var = [];
for i = 1:length(integer_variables)
    int_var(i) = find(all_variables == integer_variables(i));
end
semi_var = [];
for i = 1:length(semi_variables)
    semi_var(i) = find(all_variables == semi_variables(i));
end

if ~isempty(intersect(y_var,bin_var))
   error('Only LPs or convex QPs allowed as inner problem (inner variables can not be binary)');
end
if ~isempty(intersect(y_var,int_var))
   error('Only LPs or convex QPs allowed as inner problem (inner variables can not be integer)');
end
if ~isempty(intersect(y_var,semi_var))
   error('Only LPs or convex QPs allowed as inner problem (inner variables can not be semi-continuous)');
end
inner_p.binary_variables = bin_var;
outer_p.binary_variables = bin_var;
inner_p.integer_variables = int_var;
outer_p.integer_variables = int_var;
inner_p.semicont_variables = semi_var;
outer_p.semicont_variables = semi_var;

% Number of inequalities in inner model = #bounded dual variables
ninequalities = inner_p.K.l;
nequalities   = inner_p.K.f;

% Add dual related to inequalities in inner model to the model
dual_var = length(all_variables)+1:length(all_variables)+ninequalities;
%dual_var = length(inner_p.c)+1:length(inner_p.c)+ninequalities;

% Add dual related to inequalities in inner model to the model
eqdual_var = dual_var(end)+1:dual_var(end)+inner_p.K.f;

% No cost of duals in outer objective
p = outer_p;
if ~isempty(dual_var)
    p.c(dual_var(end))=0;
    p.Q(dual_var(end),dual_var(end)) = sparse(0);
end

if ~isempty(eqdual_var)
    p.c(eqdual_var(end))=0;
    p.Q(eqdual_var(end),eqdual_var(end)) = sparse(0);
end

% Structure of the constraints
%
%  Stationary equalities
%  Outer equalities
%  Inner equalities
%  Inner LP inequalities
%  Duals positive
%  Outer inequalities (LP, SOCP, SDP)

% Add stationarity to outer model
stationary = [inner_p.c(y_var) 2*inner_p.Q(y_var,:)];
if length(dual_var)>0
    stationary = [stationary -inner_p.F_struc(1+inner_p.K.f:inner_p.K.f+inner_p.K.l,1+y_var)'];
end
if length(eqdual_var)>0
    stationary = [stationary  -inner_p.F_struc(1:inner_p.K.f,1+y_var)'];
end

p.F_struc = [stationary;p.F_struc spalloc(size(p.F_struc,1),length(dual_var) + length(eqdual_var),0)];
p.K.f = p.K.f + length(y_var);

% Add dual>0 to outer model
p.F_struc = [p.F_struc(1:p.K.f,:);spalloc(ninequalities,length(x_var)+length(y_var)+1,0) speye(ninequalities) spalloc(ninequalities,nequalities,0);p.F_struc(1+p.K.f:end,:)];
p.K.l = p.K.l + ninequalities;

% Add inner level constraints to outer model
p.F_struc = [p.F_struc(1:p.K.f,:);inner_p.F_struc spalloc(ninequalities+nequalities,ninequalities+nequalities,0);p.F_struc(1+p.K.f:end,:)];
p.K.f = p.K.f + inner_p.K.f;
p.K.l = p.K.l + inner_p.K.l;
slack_index = p.K.f+1:+p.K.f+ninequalities;

%p.lb = outerinner_p.lb;
%p.ub = outerinner_p.ub;
p.lb(dual_var) = 0;
p.ub(dual_var) =  inf;
p.lb(eqdual_var) = -inf;
p.ub(eqdual_var) =  inf;
p.x0 = [];

%p.variabletype = outerinner_p.variabletype;
%p.monomtable   = outerinner_p.monomtable;
%p.evalMap = outerinner_p.evalMap;
%p.evalVariables = outerinner_p.evalVariables;
for i = 1:length(dual_var)
    p.monomtable(dual_var(i),dual_var(i))=1;
    p.variabletype(dual_var(i)) = 0;
end
for i = 1:length(eqdual_var)
    p.monomtable(eqdual_var(i),eqdual_var(i))=1;
    p.variabletype(eqdual_var(i)) = 0;
end

% xy = sdpvar(length(x_var)+length(y_var),1);
% z = sdpvar(length(dual_var),1);
% res = p.F_struc*[1;xy;z]
%
% F_bilevel = [res(1:p.K.f) == 0,res(p.K.f+1:end)>0]
%

% Enable outer problem to be nonconvex etc
p = build_recursive_scheme(p);
% Turned off, generates crash. Unit test in test_bilevel_1
% p = compress_evaluation_scheme(p);

p.lower = -inf;
p.options.verbose = max([0 options.verbose-1]);
p.level = 0;
p.as_free = true(ninequalities,1);
list{1} = p;
lower = -inf;
upper = inf;
iter = 0;
tol = 1e-8;
ndomcuts = 0;
ninfeascuts = 0;

% Extract the inequalities in the inner problem. These are really the
% interesting ones
inner_p.F_struc = [inner_p.F_struc(1+inner_p.K.f:end,:) spalloc(inner_p.K.l,ninequalities+nequalities,0)];

if options.verbose
    disp('* Starting YALMIP bilevel solver.');
    disp(['* Outer solver   : ' outer_p.solver.tag]);
    disp(['* Inner solver   : ' inner_p.solver.tag]);
    disp(['* Max iterations : ' num2str(p.options.bnb.maxiter)]);
    disp(' Node       Upper       Gap(%)      Lower    Open');
end
gap = inf;
xsol = [];
sol.problem = 0;
iter = 0;
inner_p = detectdisjoint(inner_p);

while length(list)>0 & gap > options.bilevel.relgaptol & iter < options.bilevel.maxiter
    iter = iter + 1;
    [p,list,lower] = select(list);

    Comment = '';
    if p.lower<upper

        if strcmp(p.options.solver,'bmibnb') & ~isinf(upper)
            % Allow early termination in bmibnb if it is used in outer
            % probllem
            p.options.bmibnb.lowertarget = upper;
        end

        output  = feval(p.solver.call,p);

        if output.problem==2
            Comment = 'Unbounded node';
        end
        if output.problem==1
            % Infeasible
            ninfeascuts = ninfeascuts + 1;
            Comment = 'Infeasible in solver';
        else

            switch output.problem
                case 0
                    Comment = 'Solved to optimality';
                otherwise
                    Comment = yalmiperror(output.problem);
            end

            z = apply_recursive_evaluation(p,output.Primal);
            cost = z'*p.Q*z + p.c'*z + p.f;

            ActuallyFeasible = checkfeasiblefast(p,z,options.bilevel.feastol);
            if ~ActuallyFeasible
                % Try to solve the relaxed feasibility problem using the
                % inner solver (i.e. treat as LP. If infeasible, it is for
                % sure infeasible
                pAux = p;pAux.c = p.c*0;pAux.Q = p.Q*0;
                outputCheck = feval(inner_p.solver.call,pAux);
                if outputCheck.problem == 1
                    % We will not continue branching, and let the user now
                    % that this choice
                    Comment = ['Infeasible'];
                    cost = inf;
                    sol.problem = 4;
                else
                    % Hard to say anything
                    Comment = ['Infeasible solution returned, resolve => continue'];
                    sol.problem = 4;
                    cost = p.lower;
                end
            end
            if cost<inf

                 if strcmp(p.options.solver,'bmibnb')
                     if output.problem == -6
                         sol.problem = -6;
                         sol.info = yalmiperror(-6);
                         info = [];
                         return
                     end
                     p.lb = max([p.lb output.extra.propagatedlb],[],2);
                     p.ub = min([p.ub output.extra.propagatedub],[],2);
                 end

                % These are duals in the original inner problem
                lambda = output.Primal(dual_var);
                % Constraint slacks in original inner problem
                slack = inner_p.F_struc*[1;output.Primal];
                % Outer variables
                xi = z(x_var);
                % Inner variables
                yi = z(y_var);

                res = (slack).*lambda;
                if ActuallyFeasible
                    res = (slack).*lambda;
                else
                    % Generate a dummy residual, to make sure we branch on
                    % the first free
                    res = (slack).*lambda*0;
                    res(find(p.as_free)) = 1:length(find(p.as_free));
                end
                if (all(p.as_free==0) | max(abs(res(p.as_free)))<options.bilevel.compslacktol) & ActuallyFeasible
                    % Feasible!
                    if upper>cost
                        upper = cost;
                        xsol = xi;
                        zsol = yi;
                        dualsol = output.Primal(dual_var);
                    end
                elseif cost>upper-1e-10
                    ndomcuts = ndomcuts + 1;
                else

                    % No official code, just playing around
                    if  ActuallyFeasible & options.bilevel.solvefrp
                        FRP = FRP0;
                        if 0
                            FRP = fixvariables(FRP0,x_var,xi,y_var);
                        else
                            FRP.F_struc = [xi -sparse(1:length(x_var),x_var,ones(length(x_var),1),length(x_var),length(x_var)+length(y_var));FRP.F_struc];
                            FRP.K.f = FRP.K.f + length(xi);
                            FRP.options.verbose = 0;
                            QQ = sparse(FRP0.Q);
                            cc = sparse(FRP0.c);
                            FRP.c(y_var) = FRP.c(y_var) + 2*FRP.Q(x_var,y_var)'*xi;
                            FRP.Q(x_var,y_var)=0;
                            FRP.Q(y_var,x_var)=0;
                            FRP.Q(x_var,x_var)=0;
                        end
                        outputFRP = feval(inner_p.solver.call,FRP);
                        if outputFRP.problem == 0
                            if 0
                                z = zeros(length(outer_p.c),1);
                                z(x_var) = xi;
                                z(y_var) = outputFRP.Primal;
                                z2 = apply_recursive_evaluation(p,z);
                            else
                                z2 = apply_recursive_evaluation(p,outputFRP.Primal);
                            end
                            costFRP = z2'*outer_p.Q*z2 + outer_p.c'*z2 + outer_p.f;
                            if costFRP < upper & isfeasible(outer_p,z2)
                                upper = costFRP;
                                xsol = z2(x_var);
                                zsol = z2(y_var);
                            end
                        end
                    end

                    [ii,jj_tmp] = max(res(p.as_free));
                    ind_tmp = (1:length(res))';
                    ind_tmp = ind_tmp(p.as_free);
                    jj = ind_tmp(jj_tmp);

                    if strcmp(p.options.solver,'bmibnb')
                        % Since BMIBNB solves a relaxation of relaxation, it
                        % can generate a lower bound which is lower than
                        % the lower bound before a compl. slack constraint
                        % was added.
                        p.lower = max(output.lower,lower);
                    else
                        p.lower = cost;
                    end

                    if iter<=options.bilevel.rootcuts
                        % Add a disjunction cut
                        p = disjunction(p,dual_var(jj),inner_p.F_struc(jj,:),output.Primal);
                        % Put in queuee, it will be pulled back immediately
                        list = {list{:},p};
                    else

                        p1 = p;
                        p2 = p;

                        % Add dual == 0 on p1
                        p1.K.f = p1.K.f + 1;
                        p1.F_struc = [zeros(1,size(p1.F_struc,2));p1.F_struc];
                        p1.F_struc(1,1+dual_var(jj))=1;
                        p1.lb(dual_var(jj)) = -inf;
                        p1.ub(dual_var(jj)) = inf;
                        newequality = p1.F_struc(1,:);
                        redundantinequality = findrows(p1.F_struc(p1.K.f+1:end,:),newequality);
                        if ~isempty(redundantinequality)
                            p1.F_struc(p1.K.f+redundantinequality,:)=[];
                            p1.K.l = p1.K.l-length(redundantinequality);
                        end

                        % Add slack == 0
                        p2.K.f = p2.K.f + 1;
                        newequality = inner_p.F_struc(jj,:);
                        p2.F_struc = [newequality;p2.F_struc];
                        redundantinequality = findrows(p2.F_struc(p2.K.f+1:end,:),newequality);
                        if ~isempty(redundantinequality)
                            p2.F_struc(p2.K.f+redundantinequality,:)=[];
                            p2.K.l = p2.K.l-length(redundantinequality);
                        end

                        p1.as_free(jj) = false;
                        p2.as_free(jj) = false;
                        if ~isempty(inner_p.disjoints)
                            here = find(inner_p.disjoints(:,1) == j);
                            if ~isempty(here)
                                p1.as_free(inner_p.disjoints(here,2))=false;
                                p2.as_free(inner_p.disjoints(here,2))=false;
                            else
                                here = find(inner_p.disjoints(:,2) == j);
                                if ~isempty(here)
                                    p1.as_free(inner_p.disjoints(here,1))=false;
                                    p2.as_free(inner_p.disjoints(here,1))=false;
                                end
                            end
                        end

                        p1.level = p.level+1;
                        p2.level = p.level+1;
                        list = {list{:},p1};
                        list = {list{:},p2};
                    end
                end
            end
        end
    else
        ndomcuts = ndomcuts + 1;
    end

    [list,lower] = prune(list,upper);

    gap = abs((upper-lower)/(1e-3+abs(upper)+abs(lower)));
    if isnan(gap)
        gap = inf;
    end
    if options.verbose
        fprintf(' %4.0f : %12.3E  %7.2f   %12.3E  %2.0f  %s\n',iter,full(upper),100*full(gap),full(lower),length(list),Comment)
    end
end
info.upper = upper;
info.iter = iter;
info.ninfeascuts = ninfeascuts;
info.ndomcuts = ndomcuts;

if ~isempty(xsol)
    assign(recover(all_variables(x_var)),xsol);
    assign(recover(all_variables(y_var)),zsol);
else
    sol.problem = 1;
end

function [list,lower] = prune(list,upper)

l = [];
for i = 1:length(list)
    l = [l list{i}.lower];
end
j = find(upper > l+1e-10);
list = {list{j}};
if length(list) == 0
    lower = upper;
else
    lower = min(l(j));
end

function [p,list,lower] = select(list)
l = [];
for i = 1:length(list)
    l = [l list{i}.lower];
end
[i,j] = min(l);

p = list{j};
list = {list{1:j-1},list{j+1:end}};
lower = min(l);

function p = addzero(p,i);

p.K.f = p.K.f + 1;
p.F_struc = [zeros(1,size(p.F_struc,2));p.F_struc];
p.F_struc(1,1+i)=1;

function outer_p = pad(outer_p,all_variables)

[i,loc] = find(ismember(all_variables,outer_p.used_variables));
p = outer_p;
% Set all bounds to infinite, and then place the known bounds
p.lb = -inf(length(all_variables),1);
p.lb(loc) = outer_p.lb;
p.ub = inf(length(all_variables),1);
p.ub(loc) = outer_p.ub;

% Set all variables as linear
p.variabletype = zeros(1,length(all_variables));
p.variabletype(loc) = outer_p.variabletype;

p.c = spalloc(length(all_variables),1,0);
p.c(loc) = outer_p.c;

if ~isempty(p.F_struc)
    p.F_struc = spalloc(size(p.F_struc,1),length(all_variables)+1,nnz(p.F_struc));
    p.F_struc(:,1) = outer_p.F_struc(:,1);
    p.F_struc(:,1+loc) = outer_p.F_struc(:,2:end);
end

% if ~isempty(p.binary_variables)
% end

p.Q = spalloc(length(all_variables),length(all_variables),nnz(outer_p.Q));
p.Q(loc,loc) = outer_p.Q;

outer_p = p;

function p = disjunction(p,variable,const,xstar)
neq = p.K.f+1;

x = sdpvar(length(p.c),1);
e = p.F_struc*[1;x];
Model1 = [x(variable)==0,-e(1:p.K.f)==0, e(1+p.K.f:end)>=0];
Model2 = [const*[1;x]==0,-e(1:p.K.f)==0, e(1+p.K.f:end)>=0];
Ab1 = getbase(sdpvar(Model1));
Ab2 = getbase(sdpvar(Model2));
b1 = -Ab1(:,1);
A1 =  Ab1(:,2:end);
b2 = -Ab2(:,1);
A2 = Ab2(:,2:end);

% b1c = [0;-p.F_struc(:,1)];
% b2c = [const(1);-p.F_struc(:,1)];
% A1c = [-eyev(length(p.c),variable)';p.F_struc(:,2:end)];
% A2c = [-const(2:end);p.F_struc(:,2:end)];
%norm(b1-b1c)
%norm(b2-b2c)
%norm(A1-A1c,inf)
%norm(A2-A2c,inf)

alpha = sdpvar(length(xstar),1);
beta = sdpvar(1);
mu1 = sdpvar(length(b1),1);
mu2 = sdpvar(length(b2),1);

Objective  = alpha'*xstar-beta;
Constraint = [alpha' == mu1'*A1,alpha' == mu2'*A2,beta <= mu1'*b1, beta <= mu2'*b2,mu1(neq+1:end)>0,mu2(neq+1:end)>0];
%Constraint = [alpha' == mu1'*A1,alpha' == mu2'*A2,beta == mu1'*b1, beta == mu2'*b2,mu1(neq+1:end)>0,mu2(neq+1:end)>0];
%Constraint = [Constraint,-10<alpha<10,sum(mu1(neq+1:end))-sum(mu1(1:neq))<10,sum(mu2(neq+1:end))-sum(mu2(1:neq))<10];
%Constraint = [Constraint,-1<alpha<1,mu1(1)+mu2(1) == 1];
Constraint = [Constraint,-1<alpha<1,sum(mu1)+sum(mu2)==1];
%Constraint = [Constraint,sum(mu1(neq+1:end))-sum(mu1(1:neq))<10,sum(mu2(neq+1:end))-sum(mu2(1:neq))<10];

solvesdp(Constraint,Objective,sdpsettings('verbose',0));
p.K.l = p.K.l + 1;
p.F_struc = [p.F_struc;-double(beta) double(alpha)'];

function p = disjunctionFAST(p,variable,const,xstar)
neq = p.K.f+1;
n = length(p.c);

b1 = [0;-p.F_struc(:,1)];
b2 = [const(1);-p.F_struc(:,1)];
A1 = [-eyev(length(p.c),variable)';p.F_struc(:,2:end)];
A2 = [-const(2:end);p.F_struc(:,2:end)];

alpha_ind = 1:length(xstar);
beta_ind = alpha_ind(end)+1;
mu1_ind = (1:length(b1))+beta_ind;
mu2_ind = (1:length(b2))+mu1_ind(end);

alpha = sdpvar(length(xstar),1);
beta = sdpvar(1);
mu1 = sdpvar(length(b1),1);
mu2 = sdpvar(length(b2),1);

p_hull = p;
p_hull.c = zeros(mu2_ind(end),1);
p_hull.c(alpha_ind) = xstar;
p_hull.c(beta_ind) = -1;

% equalities alpha = Ai'*mui, sum(mu)==1
p_hull.K.f = length(xstar)*2+1;
p_hull.F_struc = [zeros(length(xstar),1) eye(length(xstar)) zeros(length(xstar),1) -A1' zeros(length(xstar),length(b2))];
p_hull.F_struc = [p_hull.F_struc;
    zeros(length(xstar),1) eye(length(xstar)) zeros(length(xstar),1) zeros(length(xstar),length(b2)) -A2'];
p_hull.F_struc = [p_hull.F_struc ;1 zeros(1,length(xstar)) 0 -ones(1,length(b1)+length(b2))];

% Inequalities
p_hull.F_struc = [p_hull.F_struc ;0 zeros(1,length(xstar)) -1 b1' b2'*0];
p_hull.F_struc = [p_hull.F_struc ;0 zeros(1,length(xstar)) -1 0*b1' b2'];
npmu = length(b1)-neq;
p_hull.F_struc = [p_hull.F_struc; zeros(npmu,1) zeros(npmu,length(xstar)) zeros(npmu,1) zeros(npmu,neq) eye(npmu) zeros(npmu,length(b2))];
p_hull.F_struc = [p_hull.F_struc; zeros(npmu,1) zeros(npmu,length(xstar)) zeros(npmu,1) zeros(npmu,length(b1)) zeros(npmu,neq) eye(npmu)];
p_hull.F_struc = [p_hull.F_struc; ones(length(xstar),1) -eye(length(xstar)) zeros(length(xstar),1+2*length(b1))];
p_hull.F_struc = [p_hull.F_struc; ones(length(xstar),1)  eye(length(xstar)) zeros(length(xstar),1+2*length(b1))];
p_hull.K.l = 2+npmu*2+2*length(xstar);
p_hull.lb = [];
p_hull.ub = [];
output = feval(p_hull.solver.call,p_hull);
alpha = output.Primal(alpha_ind);
beta = output.Primal(beta_ind);

% Objective  = alpha'*xstar-beta;
% Constraint = [alpha' == mu1'*A1,alpha' == mu2'*A2,beta <= mu1'*b1, beta <= mu2'*b2,mu1(neq+1:end)>0,mu2(neq+1:end)>0];
% Constraint = [Constraint,-1<alpha<1,sum(mu1)+sum(mu2)==1];
% Constraint = [Constraint,sum(mu1)+sum(mu2)==1];
% solvesdp(Constraint,Objective,sdpsettings('verbose',0));

p.K.l = p.K.l + 1;
p.F_struc = [p.F_struc;-double(beta) double(alpha)'];

function feas = isfeasible(p,x)

feas = checkfeasiblefast(p,x,1e-8);

function p = detectdisjoint(p);

p.disjoints = [];
% for i = 1:p.K.l
%     row1 = p.F_struc(i+p.K.f,:);
%     for j = 2:1:p.K.l
%         row2 = p.F_struc(j+p.K.f,:);
%
%         if all(abs(row1)-abs(row2)==0)
%             % candidate
%             if nnz(row1 == -row2 & row1~=0)==1
%                 p.disjoints = [p.disjoints;i j];
%             end
%         end
%     end
% end
%

function FRP = fixvariables(FRP0,x_var,xi,y_var);
% Copy current model
FRP = FRP0;

%
FRP.c(y_var) = FRP.c(y_var) + 2*FRP.Q(x_var,y_var)'*xi;
FRP.c(x_var) = [];
FRP.Q(:,x_var) = [];
FRP.Q(x_var,:) = [];
FRP.lb(x_var) = [];
FRP.ub(x_var) = [];
B = FRP.F_struc(:,1+x_var);
FRP.F_struc(:,1+x_var)=[];
FRP.F_struc(:,1) = FRP.F_struc(:,1) + B*xi;

%                         FRP.F_struc = [xi -sparse(1:length(x_var),x_var,ones(length(x_var),1),length(x_var),length(x_var)+length(y_var));FRP.F_struc];
%                         FRP.K.f = FRP.K.f + length(xi);
%                         FRP.options.verbose = 0;
%                         QQ = FRP0.Q;
%                         cc = FRP0.c;
%                         FRP.c(y_var) = FRP.c(y_var) + 2*FRP.Q(x_var,y_var)'*xi;
%                         FRP.Q(x_var,y_var)=0;
%                         FRP.Q(y_var,x_var)=0;
%                         FRP.Q(x_var,x_var)=0;

function [merged_mt,merged_vt] = mergemonoms(inner_p,outer_p);

if isequal(inner_p.used_variables,outer_p.used_variables)
    merged_mt = inner_p.monomtable;
    merged_vt = inner_p.variabletype;
else
    invar  = inner_p.used_variables;
    outvar = outer_p.used_variables;
    all_variables = unique([invar outvar]);

    [i_inner,loc_inner] = find(ismember(all_variables,inner_p.used_variables));
    [i_outer,loc_outer] = find(ismember(all_variables,outer_p.used_variables));

    merged_mt = spalloc(length(all_variables),length(all_variables),0);
    merged_vt = zeros(1,length(all_variables));

    for i = 1:length(i_inner)
        [ii,jj,kk] = find(inner_p.monomtable(i,:));
        merged_mt(loc_inner(i),loc_inner(jj)) = kk;
        merged_vt(loc_inner(i)) = inner_p.variabletype(i);
    end
    for i = 1:length(i_outer)
        [ii,jj,kk] = find(outer_p.monomtable(i,:));
        merged_mt(loc_outer(i),loc_outer(jj)) = kk;
        merged_vt(loc_outer(i)) = outer_p.variabletype(i);
    end
end


function  [cct, ta1, tetael] = srcint1(la, nn1, ct4, pp1, pp2, ctc0, ctad, nprimtilt, lmax, K, stfname)
% srcint1.m integrates the source including polarisation over conformal time using the approach of Seljak :
% integration over the photon past light cone, see M Zaldarriaga et al., ApJ nr 494, 491 (1998) for K > 0
% the function needs an l-range (la), a k-range (nn1) a ctime-range (ct4), a series of splines of the source
% in conformal time for the temperature (pp1) and for the polarisation (pp2),the present conformal time (ctc0),
% decoupling time (ctad), the index of the primary power spectrum (nprimtilt),
% the maximum la of the anistropy spectrum (lmax) and the curvature energy content from the Friedman equation (K),
% the startfile name of the ultra-spherical function.
%
% the result is the square of the temperature spectrum (1) resp the E-polarisation spectrum (2)
% resp the cross-correlation of temperature and polarisation (3).
%
% D Vangheluwe 13 june 2005
% remark 1a: attention :nn1 is an integer array :wavenumber array k1 is made from it by taking k1 = nn1 * sqrt(K)
% remark 1: we use ultra-spherical bessel values which have two parameters and must be found by integration:
%   for the calculation of the ultra-spherical bessel functions see: cmb/usphint.m
% remark 2: for the ode equation of the u-function, see equation (36) of Zaldarriaga & Seljak.
% remark 3: take attention the value of nn1 should lie within the range of kb1 values : there is no check!!
%   see spline of start  values x10 and pbd0.
% remark 4: how do we get the ultra-spherical bessel function values? see development and test routine usphpar.m
%  and usphpar1.m
% remark 5: this function is a fast version of srcpint.m (the source need not to be in the loop
% if there is enough memory, probably 80Mbytes will be used)
% remark6 on 13 june: this routine has been obtained by modifying srcpintf.m according to usphpar1.m

if (K < 0),
   error('the space curvature constant K should be > 0 for this routine');
   return;
end
kk0 = sign(K);
lla = size(la, 2);
k1 = nn1 * sqrt(K);
lk1 = size(k1, 2);
lct4 = size(ct4, 2);
k1step = k1(2) - k1(1);
ct4step = ct4(2) - ct4(1);
k1max = k1(lk1);
k1min = k1(1);
ct4sym = 0.5 * pi/sqrt(K);

% if necessary adapt the source to the period of the ultra-spherical function for K >0
ct0 = ct4(1);
ict1 = 1 : lct4;
if ct4sym < ct4(end)
   ctc4 = ctc0 - ct4;
   ict1 = find(ctc4 < ct4sym);
   lct1 = size(ict1,2);
%   ct0 = ctc0 - ct4sym;
   ct0 = ct4(ict1(1));
% map the region ctc4 >= ct4sym onto ict1 by mirroring it wrt the point: ctc4 = ct4sym
   ict2 = find(ctc4 < ct4sym  &  ctc4 > 2*ct4sym - ctc0 + ct4step);
% values of ct4 to be used to calculate (by interpolation) the mirrored source values:
   ct44 = 2*(ctc0 - ct4sym) - ct4(ict2);
% define a (small) region (index icts) where the source changes rapidely and should be splined for reconstruction:
% ct4(icts(1..end)) is projected onto ct44s(end..1) :notice that the index is inverted, the phaseshift between
% the two amounts to : 2*rem(ctc0 - ct4sym - ct4(1), ct4step).
% The same phaseshift and index inversion exists also between ct4(ict1(1..end) and ct44(end..1)
% icts and ict3 have the same size and take attention that they can be empty for ctc0 - ctad +100 < ct4sym!!
   ctcsmin = 2*ct4sym - ctc0 + (ctad - 100);
   ctcsmax = 2*ct4sym - ctc0 + (ctad + 100);
   icts = find((ct4 < ctad + 100)  &  (ct4 >= ctad - 100)  &  (ctc0 - ct4 >= ct4sym));
   ict3 = find(ctc4(ict2) < ctcsmax  &  ctc4(ict2) >= ctcsmin);
   if ~isempty(ict3),    ct44s = ct44(ict3); end
end

% load the table and find the start values for the integration of the ode for u-functions
% the startfile can be obtained by running usphst.m
%  stv1 = load('usphst.dat');
stv1 = load(stfname);
lmax0 = stv1.ust.la(end);

nrsteps = 10000;   % default 10000
%lmax = 1500;
xmax = k1(lk1) * ctc0;   % =default 3000
xstep = 2*lmax0/nrsteps;
kx = 1e-12 : xstep : xmax;

% allocate the data in order to speed up the routine
kctc0 = zeros(1, lct4);
kctc = zeros(1, lct4);
jl = zeros(lct4, 1);
htable = zeros(lct4, 1);
cmbtable = zeros(1, lk1);

% calculate the curvature length times wavenumber : a parameter for the ultra_spherical bessel function
kb = sqrt(abs(K)) ./k1;

%#########
%kb(1)
%kb(end)
if all(kb == 0), kbzero = 1; x10 = 1e-12 * ones(1, lk1);
else  % not all kb are zero

   kbzero = 0;
   ikb = find(kb > 10);
   if ~isempty(ikb),
      kb(ikb) = 10 * ones(1, size(ikb, 2));
      message('boundary kb > 10 reached')
   end

% split the structure stv1 from the startfile : x1: start values where pbi_beta = 1e-6,
% pbd : dphi_beta/dx at the start values, all data exact within 1e-6.
% the la-values resp kb-vector should be the same as in the program usphst.m
   la1 = stv1.ust.la;
   lla1 = size(la1, 2);
   kb2 = stv1.ust.kb;
   x1v1 = stv1.ust.x1;
   pbdv1 = stv1.ust.pbd;

end %all(kb == 0)

%############################# start
for il = 1:lla

    l = la(il)
    laa = sqrt(l * (l + 1));

% make a range of wavenumber and kb starting at 1/(l+1)
    clear('k2')
%    ik2 = (l+1) : lk1;
    ik2 = find(nn1 >= l+1);
    k2 = k1(ik2);
    lk2 = size(k2, 2);
    nn2 = nn1(ik2);
    cmbtable = zeros(1,lk2);

% the range of indices in src for k2 will be ik2 : take care, do not neglect!!:
    k2min = k2(1);
    k2max = k2(lk2);
    tetatl = zeros(1, lk2);
    tetael = zeros(1, lk2);
    kb = sqrt(abs(K)) ./k2;
    odd_la = xor(rem(nn2,2), rem(l,2));

% the kb-range is made more progressive towards 1/la
    nrkbsteps = 100;
    kbmax = 1/laa;
    ekbmax = -7;
    ekbmin = log(kbmax - 1e-5)/log(10);
    ekbstep = (ekbmax - ekbmin)/nrkbsteps;
    ekb1 = ekbmin : ekbstep : ekbmax;
    kb1 = kbmax - 10 .^ekb1;
    lkb1 = size(kb1, 2);
    if l == la1(1)
        if any(kb1 ~= kb2)  error('kb vector is unequal to the start vector'); return; end
    end

% find the start values (hs) for the ode integration step as a function of la and kb (is a surface):
% the long sought magic formula : la1 is the la-vector from usphst1.m!!
    hs = 0.5 * x1v1 .* repmat((la1 .^-0.9)', 1, lkb1);
    if ~kbzero
       il1 = find(la1 == l);
       if isempty(il1)
           error('la not in the range of la1')
           break;
       else
           x10 = spline(kb1, x1v1(il1,:), kb);
           pbd0 = spline(kb1, pbdv1(il1,:), kb);
           hstart = spline(kb1, hs(il1,:), kb);
       end

% calculate the start values for the u-function : u0 = [u, du/dx] with u = r(x) * phi_beta(x) and
%  du/dx = phi_beta(x) * dr(x)/dx + r(x) * dphi_beta(x)/dx :
       r10 = sin(kb .* x10) ./kb;
       rd10 = cos(kb .* x10);
       u0 = [r10 *1e-6; r10 .* pbd0 + 1e-6 * rd10];

% define the step and set 'odeint' to a constant number of steps for all cases (kb)
       toi = 1e-4;
       hmax = 0.3;   %we take hmax = 0.3 as the default value
       maxstp = 150;   % default maxstp = 150
       x1e = ones(1, lk2) * xmax;
% solve the u-function instead of the phi_beta function : xs2 does not have a constant step
       [xs2, u2, nok, nbad, nfev] = odeintp(u0, x10, x1e, toi, hstart, 0, hmax, maxstp, @uspheq1, l, kb, kk0);

% prepare a spline of the solution for a constant step in xs (the values are found with ppval1):
       y2der = splinep(xs2, u2);
% calculate the number of constant steps, xstep in the solution xs2: ilast is the last one
%       ilast = ceil((xs2(end,:) - x10)/xstep);
% set the number of overlapping steps for the matching to the asymptotic solution (default= 40):
       noverflow = 85;
       ncsteps = 0;

    end  % kbzero

% calculate also the spherical bessel function as we may need it for kb < 1e-5
    table_bv = sphbes(l, kx);
% set some constants needed for the polarisation formula
    gl = sqrt((l + 2) * (l + 1) * l * (l - 1));

% interpolate and integrate (sum with the Simpson rule) over conformal time :
    for i = 1:lk2
%    for i = 26:26

       clear('htable', 'src4', 'src5')
       src4 = ppval(pp1, k2(i));
       src5 = ppval(pp2, k2(i));

       if kb(i) > 1e-5 * (1500/l)

% define a vector of argument values for the ultra-spherical bessel function (x1): reverse ct4
%          clear('xs', 'x1', 'x2', 'yspl2','iover')
          x1 = x10(i) : xstep : xmax;
% define some parameters for the calculation ; x1=x1sym up to the symmetry point, x1* kb = pi/2 needed for K>0 :
          x1sym = min(0.5*pi/kb(i), xmax);
% in the next 10 lines make a table over x1 (ul) of ultra-spherical bessel values:

          if  xs2(end,i) >=  x1sym

% asymptotic expansion is not needed here as we found already the complete solution: include one point xs2 > x1sym+xstep
% adding xs2step + xstep to x1sym in the find logic statement of xs2 makes sure that xs(end) > x1(ilast) > x1sym
             xs2step = max(diff(xs2(:,i)));
             ix2 = find(xs2(:,i) <= (x1sym + xs2step + xstep));
% include one point xs2 > x1sym in the interpolation of the ode solution:
%             ix2 = [ix2', (ix2(end) + 1)];
             [xs, yspl2] = ppval1(xs2(ix2,i), u2(ix2,i), y2der(ix2,i), xstep);
%             ilast = floor((x1sym - x10(i))/xstep) + 1;
             ilast = floor((x1sym - x10(i))/xstep) + 2;

          else  % all steps in odeint are used (full interpolation) : an asymptotic solution is needed

             [xs, yspl2] = ppval1(xs2(:,i), u2(:,i), y2der(:,i), xstep);
             ilast = floor((xs(end) - x10(i))/xstep) + 1 - ncsteps;
% define the overflow index (iover): default 85 (or 40) steps should at least include one zero and one maximum of u2
             iover = (ilast - noverflow) : ilast;

% find the asymptotic values and the phase correction in the overflow region:
             dphi = phdif(x1(iover), usphas(l, kb(i), x1(iover), 0, 0, kk0), yspl2(iover));

          end

% calculate the ultra-spherical bessel values
          kctc0 = ctc0 * k2(i) * ones(1, lct4);
          kctc = k2(i) * ct4;
          xctc = kctc0 - kctc;
% xctc > x1(1) is necessary as x1(1)=x10 is not the origin of ct, but the point where odeint started
          if   xs2(end,i) >=  x1sym
% debug action 12-6-2005 : interpolation over xs limits our range to xs(end) at most if it happens xs(end) < x1sym
%             ixode = find(xctc > x1(1)  &  xctc <= x1sym);
             ixode = find(xctc > x1(1)  &  xctc <= min(x1sym, xs(end)));
          else
             ixode = find(xctc > x1(1)  &  xctc <= x1(ilast));
          end
          ul = zeros(1, lct4);
          if  ~isempty(ixode)
             r1 = sin(kb(i) * xs)/kb(i);

% interpolate the ultra-spherical bessel function when we are in the k-range of the ode solution
             ul(ixode) = interpl(xs, yspl2 ./ r1, xctc(ixode));
% calculate the necessary values of the asymptotic expansion of the ultra-spherical bessel function
             if  (xs2(end,i) <  x1sym)   % asymptotic expansion needed for K>0
                ixas = find(xctc > x1(ilast)  &  xctc <= x1sym);
                ul(ixas) = usphas1(l, kb(i), xctc(ixas), dphi, 1, kk0);
             end

% if necessary adapt the source to the period of the ultra-spherical function for K >0 by mirroring:
% we should have: find(k2(i)* (ctc0 - ct4(ict1)) < x1sym) == find(xctc < x1sym) == ict1
             if  ct4sym < ct4(end)
% reconstruct the source by linear interpolation : not so accurate but sufficient where changes are slow
                src42 = interpl(ct4, src4', ct44)';
                src52 = interpl(ct4, src5', ct44)';
% for efficieny reason reconstruct the source by splines only in the region (ict3) where it changes fast
                if ~isempty(ict3)
                  src42(ict3) = spline(ct4(icts), src4(icts), ct44s)';
                  src52(ict3) = spline(ct4(icts), src5(icts), ct44s)';
                end
                if odd_la(i) == 1
                   src4(ict2) = src4(ict2) + src42;
                   src5(ict2) = src5(ict2) + src52;
                else
                   src4(ict2) = src4(ict2) - src42;
                   src5(ict2) = src5(ict2) - src52;
                end
             end

debug = 0;
if debug == 1
ct4step
nn2(i)
ctc0-ct4sym
ul(ict1(1))
src4(ict1(1))
ct0
odd_la(i)
x10(i)
xs2(end,i)
xs(1)
xs(end)
x1(1)
x1(ilast)
x1sym
xstep

figure(1)
%plot(ct4(ict1), src4(ict1), ct4, ul, 'k--', ct4, src4(:,ik2(i)), 'b--', ct4(ict2(ict3)), src42(ict3),'r')
plot(ct4(ict1), src4(ict1), ct4, ul, 'k--', ct4, src4, 'b--')
%axis([6500, 7100, -0.05, 0.05])
end

% integrate the source function over ct4, following formula (40) of Zaldarriaga et al.(1998)
             htable = src4(ict1) .* ul(ict1)';
             tetatl(i) = simpsint(ct0, ct4(end), htable) + (ct0 - ctc0 + ct4sym) * src4(ict1(1)) * ul(ict1(1));
             htable = src5(ict1) .* ul(ict1)';
             tetael(i) = simpsint(ct0, ct4(end), htable) + (ct0 - ctc0 + ct4sym) * src5(ict1(1)) * ul(ict1(1));
          else
             tetatl(i) = 0;
             tetael(i) = 0;
          end

       else  %if kb(i) <= 1e-5 *(1500/l)

% interpolate the spherical bessel function
          kctc0 = ctc0 * k2(i) * ones(1, lct4);
          kctc = k2(i) * ct4;
          xctc = kctc0 - kctc;
          jl = interpl(kx, table_bv, xctc)';
% integrate the source function following (12) and (13) of Seljak resp (18) of Zaldarriaga and Seljak
% the integration interval is limited to ct4 where the source <> 0
          htable = src4 .* jl;
          tetatl(i) = simpsint(ct4(1), ct4(end), htable);
          htable = src5 .* jl;
          tetael(i) = simpsint(ct4(1), ct4(end), htable);

       end  %if kb(i)

   end  % forloop k2

ta1 = tetatl;
f2 = 1;
%if (K ~= 0),  f2 = coth(pi*k2/sqrt(abs(K))); end
factor = sqrt((k2 .^2 - 4*K) ./ (k2 .^2 - K));
%factor = 1;
% perform the integration over k2min-k2max, following (9) of Seljak resp (19) of Zaldarriaga and Seljak
   cmbtable = f2 * factor .* (tetatl .^2) .* (k2 ./ (k2 .^2 - K)) .^ (2 - nprimtilt);
%ta1 = cmbtable;
%    cct.ctt(il) = l*(l + 1) * sqrt(K) * sum(cmbtable');
    cct.ctt(il) = l*(l + 1) * k1step * sum(cmbtable');
%   cct.ctt(il) = l*(l + 1) * simpsint(k2min, k2max, cmbtable')
   cmbtable = factor .* (tetael .^2) .* (k2 ./ (k2 .^2 - K)) .^ (2-nprimtilt);
   cct.cee(il) = gl^2 * l*(l + 1) * k1step * sum(cmbtable');
   cmbtable = factor .* (tetael .* tetatl) .* (k2 ./ (k2 .^2 - K)) .^ (2-nprimtilt);
   cct.cte(il) = l*(l + 1) * gl * k1step * sum(cmbtable');
end  % forloop la