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


%% Estimate the quality of QRS complex detections
% Calculate the quality of QRS detections based on the the likelihood of
% measurements from RR interval series with respect to a trained model. See
% for further details. See 'A Pattern-Recognition Approach for
% Lead-Selection in Heartbeat Detection' CINC 2014
%
% Example
%
%   payload_out = calculateSeriesQuality(payload_out, ECG_header, start_end_this_segment)
%
% Arguments:
%   + payload_out: structure with the QRS detections series to estimate the
%       quality. One fieldname contains the QRS detections series in a
%       'time' field. For example
%       payload_out.('QRS_detector1_name_lead_I').time contains the
%       detections of QRS_detector1 in lead1. See ECGtask_QRS_detection for
%       examples.%
%   + ECG_header is a structure (ECG_header prop. in ECGwrapper object)
%       describing the signal.
%   + start_end_this_segment: array samples where starts and finish this
%       signal segment [start_sample end_sample].
%
% Output:
%   + payload_out: The same input structure with extra field
%       'series_quality' describing the quaility of the QRS detection
%       series in three fields: ratios, a measure of quality of the QRS
%       detections, estimated_labs, for internal use, and AnnNames which
%       matches the previous fields to the fieldname found in payload_out.
%
% See also ECGtask_QRS_detection
%
% Author: Mariano Llamedo Soria llamedom@electron.frba.utn.edu.ar
% Version: 0.1 beta
% Birthdate: 01/01/2012
% Last update: 18/10/2014
% Copyright 2008-2015
function payload_out = calculateSeriesQuality(payload_out, ECG_header, start_end_this_segment)

    if( isempty(payload_out) )
        return
    end

    % estimate quality of QRS detections performed
    [AnnNames, all_annotations] = getAnnNames(payload_out);

    [ ratios, estimated_labs ] = CalcRRserieRatio(all_annotations, ECG_header, start_end_this_segment);
    [ratios, best_detections_idx] = sort(ratios, 'descend');
    AnnNames = AnnNames(best_detections_idx,:);

    payload_out.series_quality.ratios = ratios;
    payload_out.series_quality.estimated_labs = estimated_labs(best_detections_idx);
    payload_out.series_quality.AnnNames = AnnNames;
    payload_out.series_quality.sampfreq = ECG_header.freq;


function ply_to_tri_surface_test02 ( ply_file_name )

%*****************************************************************************80
%
%% TEST02 tests TRI_SURFACE_WRITE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST02\n' );
  fprintf ( 1, '  PLY_TO_TRI_SURFACE reads a PLY file as TRI_SURFACE data.\n' );
  fprintf ( 1, '  TRI_SURFACE_WRITE writes the data to a pair of TRI_SURFACE files.\n' );
%
%  Read PLY data.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Extract nodes and triangles from "%s"\n', ply_file_name );

  [ node_xyz, triangle_node ] = ply_to_tri_surface ( ply_file_name );
%
%  Get sizes of the data.
%
  [ dim_num, node_num ] = size ( node_xyz );
  [ order_num, triangle_num ] = size ( triangle_node );
%
%  Print some of the data.
%
   r8mat_transpose_print_some ( dim_num, node_num, node_xyz, ...
     1, 1, dim_num, 5, '  The first 5 nodes:' );

   triangle_num = size ( triangle_node, 2 );

   i4mat_transpose_print_some ( order_num, triangle_num, triangle_node, ...
     1, 1, order_num, 5, '  The first 5 triangles.' );
%
%  Write data to TRI_SURFACE files.
%
  node_file_name = 'cube_nodes.txt';
  triangle_file_name = 'cube_elements.txt';

  tri_surface_write ( node_file_name, triangle_file_name, dim_num, ...
    node_num, order_num, triangle_num, node_xyz, triangle_node );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Graphics data written in TRI_SURFACE format to:\n' );
  fprintf ( 1, '  node file     = "%s".\n', node_file_name );
  fprintf ( 1, '  triangle file = "%s".\n', triangle_file_name );

  return
end


function yout2 = ola(yin,fragsize,overlap)

noover  = 1 - overlap;
Move    = fragsize * noover;
Nframe  = size(yin,1);
NFFT    = size(yin,2);
yout    = zeros(1, Nframe*Move+NFFT);

for n = 1:Nframe
    framestart  = 1 + (n-1)*Move;
    frameend    = fragsize + (n-1)*Move;
    yout(framestart:frameend) = yout(framestart:frameend) + yin(n,1:fragsize);
end

yout2 = yout(1:Nframe*Move+fragsize*overlap);


function f3=f3(x)
Bound=[-100 100];

if nargin==0
    f3 = Bound;
else
    f3=sum(cumsum(x).^2);
end


function geneCopyFactor = childCopyGivenFreqsFactor(alleleFreqs, geneCopyVar)
% This function creates a factor whose values are the frequencies of each
% allele in the population.
%
% Input:
%   alleleFreqs: A list of the frequencies of the alleles in the population
%   genotypeVar: The variable number for the genotype
%
% Output:
%   geneCopyFactor: A factor for the prior probability of genotypeVar (note
%   that this is the FULL CPD with no evidence observed)

numAlleles = length(alleleFreqs);
geneCopyFactor = struct('var', [], 'card', [], 'val', []);
geneCopyFactor.var(1) = geneCopyVar;
geneCopyFactor.card(1) = numAlleles;
geneCopyFactor.val = alleleFreqs';


% Copyright (C) 2018  Symeon Symeonidis, Stefanos Tsantilas, Stelios Mitilineos
% simos421@gmail.com, steftsantilas@gmail.com, smitil@gmail.com
% 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.

addpath(genpath('...\cst api'));

cst = actxserver('CSTStudio.application');
mws = cst.invoke('NewMWS');

Geometry = 'mm';
Frequency = 'GHz';
Time = 'ns';
TemperatureUnit = 'Kelvin';
Voltage = 'V';
Current = 'A';
Resistance = 'Ohm';
Conductance  = 'S';
Capacitance = 'PikoF';
Inductance = 'NanoH';

CstDefineUnits(mws,Geometry, Frequency, Time, TemperatureUnit, Voltage, Current, Resistance, Conductance, Capacitance, Inductance)
CstMeshInitiator(mws)

CstDefineFrequencyRange(mws,0.5,4)
minfrequency = 0.5;
x='expanded open';
CstDefineOpenBoundary(mws,minfrequency,x,x,x,x,x,x)
XminSpace = 0;
XmaxSpace = 0;
YminSpace = 0;
YmaxSpace = 0;
ZminSpace = 0;
ZmaxSpace = 0;
CstDefineBackroundMaterial(mws,XminSpace,XmaxSpace, YminSpace, YmaxSpace, ZminSpace, ZmaxSpace)

Name = 'Groundplane';
component = 'component1';
material = 'PEC';
Xrange = [-40 40];
Yrange = [-40 40];
Zrange = [0 2];

Cstbrick(mws, Name, component, material, Xrange, Yrange, Zrange)

Name = 'Conductor';
OuterRadius = 2;
InnerRadius = 0;
Xcenter = 0;
Ycenter = 0;
Zrange = [8 38];

Cstcylinder(mws, Name, component, material, 'Z', OuterRadius, InnerRadius, Xcenter, Ycenter, Zrange)

PortNumber = 1;
SetP1 = [0 0 2];
SetP2 = [0 0 8];

CstDiscretePort(mws,PortNumber,SetP1,SetP2)

monitorindex = 2.25;
CstDefineEfieldMonitor(mws,strcat('e-field', num2str(monitorindex)), monitorindex)
CstDefineFarfieldMonitor(mws,strcat('Farfield',num2str(monitorindex)), monitorindex)
CstSaveProject(mws)

CstDefineTimedomainSolver(mws,-30)

exportpath = '...\cst api\NameofyourFile';%NameofyourFile should be the same with the name of the file of line 90 bellow

format = 'db';

CstExportTouchstone(mws,exportpath, format)
data = read(rfdata.data,'NameofyourFile.s1p');
freq = data.Freq;
s_params = extract(data,'S_PARAMETERS',50);
spar = s_params(1,:);
s11 = 20*log10(sqrt(real(spar).^2 + imag(spar).^2));
plot(freq,s11); grid on;


function [yd3] = mi32yd3(mi3)
% Convert volume from cubic miles to cubic yards.
% Chad Greene 2012
yd3 = mi3*5451776000;


function X=sos(X,r)
%SOS Declare sum-of-squares structure
%
% F = sos(p,r)
%
% Input
%  p : SDPVAR object
%  r : Desired rank (optional)
% Output
%  F : Constraint object
%
% Example:
%  Typical usage is
%
%   F = sos(p)
%
%  An experimental feature is to search for
%  low rank decompositions. To search for a
%  decomposition using at most 3 terms, use
%  a second argument
%
%   F = sos(p,3)
%
%  Note that his feature requires the solver LMIRANK.
%
% See also SOSEX

if ~issymmetric(X)% X.n>1 | X.m>1
    error('SOS can only be applied to symmetric polynomial matrices');
end
if nargin<2
    r = inf;
end
X.typeflag = 11;
X.extra.sosid = yalmip('sosid');
X.extra.rank = r;


% JN Kather, extract microns per pixel MPP from Leica SVS
function currMPP = Info2MPP(currImInfo,channel)

    if isfield(currImInfo,'ImageDescription')
    currDescription = currImInfo(channel).ImageDescription;
    disp(currDescription);
    try
        st = strfind(currDescription,'MPP = ');         % find start
        currDescription = currDescription((st+6):end);  % strip before start
        en = strfind(currDescription,'|');      % find end
        if ~isempty(en)
        currDescription = currDescription(1:(en(1)-1));    % strip after end
        end
        currMPP = str2double(currDescription);  % get MPP (microns per pixel)
    catch
        warning('FAILED to parse description');
        currMPP = [];
    end
    else
        warning('no ImageDescription field');
        currMPP=[];
    end

end


function c = agc(c,varargin)

% This function applies automatic gain control (AGC) to each trace. This
% process, commonly used in seismic reflection processing, applies a
% variable scale to each trace such that the amplitude along the entire
% trace is roughly uniform. By minimizing amplitude variations along the
% trace, well-correlated but low-amplitude phases become more visible. A
% time window may be specified to control how tightly this scaling is
% applied. Use a longer window for lower frequecy signals. The function
% returns a correlation object.
%
% EXAMPLES:
%  c=agc(c)    				apply agc using the default time window (0.5 s)
%  c=agc(c,0.8)            	apply agc using a window of 0.8 s

% Author: Michael West, Geophysical Institute, Univ. of Alaska Fairbanks
% $Date$
% $Revision$

if (length(varargin) >= 2)
    error('Too many inputs');
end

if ~isa(c,'correlation')
    error('First input must be a correlation object');
end

if length(varargin)==1
	agcwin = varargin{1};
else
	agcwin = 0.5;
end;
agcsamp = round( agcwin * get(c.W(1),'Fs') );

% LOOP THROUGH TRACES APPLYING GAIN
for tracenum = 1:length(c.W)
   w = get(c.W(tracenum),'DATA');
   scale=zeros( length(w)-2*agcsamp , 1 );
   for index=-1*agcsamp:agcsamp
      scale=scale + abs( w(agcsamp+index+1:agcsamp+index+length(scale)) );
   end;
   scale = scale/mean(abs(scale));
   scale = [ones(agcsamp,1)*scale(1) ; scale ; ones(agcsamp,1)*scale(end)];
   w = w./scale;
   c.W(tracenum) = set(c.W(tracenum),'DATA',w);
end;


function [ know, x ] = p31_sol ( n )

%*****************************************************************************80
%
%% P31_SOL returns the solution for problem 31.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 January 2001
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the problem.  This value
%    is only needed for those problems with variable N.
%
%    Output, integer KNOW.
%    If KNOW is 0, then the solution is not known.
%    If KNOW is positive, then the solution is known, and is returned in X.
%
%    Output, real X(N), the solution, if known.
%
  know = 1;

  x = 4.0 * ones ( n, 1 );

  return
end


function [f,k,s0,gof] = relaxFitMt(data,delta,r1,s0,tr,flipAngle,fitMethod,useParfor)
%
% [f,k,gof] = relaxFitMt(data,delta,t1,s0,tr,flipAngle,[fitMethod='f'],[useParfor=false])
%
% Computes a nonlinear fit of the bound-pool (f) map.
%
% fitMethod = 'f' for fminsearch, 'l' for lsqnonlin.
%
% tr should be in seconds, flipAngle in radians. r1 is 1/t1.
%
% Returns:
%
% SEE ALSO:
%
% relaxFitT1.m to fit the t1 and pd maps used by this function.
%
% HISTORY:
% 2008.02.26 RFD: wrote it.
% 2009.12.04 RFD: changed starting values to be a closer to the typical
% white matter values. I also tightened the range of allowable values for k
% to reflect the published values for brain tissue. Both of these changes
% have the effect of reducing crazy f-values, mostly in the csf. They do
% not seem to affect resulting f-values in the white matter, except in
% regions of high noise where the fits are unstable.

if(~exist('fitMethod','var')||isempty(fitMethod))
    fitMethod = 'f';
end
if(~exist('useParfor','var')||isempty(useParfor))
    useParfor = false;
end

% RFD: empirically, [1 .08] is the median for brain tissue. But, we mostly
% care about white matter, so we'll use the typical values for white matter
x0 = [3 .10];
%x0 = [2.4, .10]'; %this is our initial guess, bese [3.4, .15]'

% lb = [.1 .03]; % [k f]
% ub = [5 .28];
% 2009.12.04 RFD: tighten the reigns on k
lb = [0.9 .03]; % [k f]
ub = [4.5 .28];

t_m = 8e-3; %bese 8e-3
t_s = 5e-3; %bese 5e-3
t_r = 19e-3; %bese 19e-3
T2_B = 11e-6;
% The following (omega_1rms) is derived from the MT RF pulse shape. We
% hard-code a value here, but we should check it's validity.
% The angular freq of the MT pulse- the equivalent flip angle of the MT
% pulse (expressed in radians/sec).
%w1rms = 2400;
w1rms = 2400; % 382Hz * 2pi = 2400

for ii = 1:length(delta)
    W_B(ii) = pi*(w1rms^2)*lorentzian(delta(ii), T2_B);
end;

if(fitMethod=='l')
   options = optimset('LargeScale','on','LevenbergMarquardt','on', 'Display', 'off', 'MaxIter', 50);
else
   options = optimset('LargeScale','off','LevenbergMarquardt','on', 'Display', 'off', 'MaxIter', 50);
end

sz = size(data);
if(sz(1)~=numel(delta))
   error('size(data,1) must = numel(delta)!');
end

f = zeros(1,sz(2));
k = zeros(1,sz(2));
gof = zeros(1,sz(2));

% What we compute here is actually W_F./R1_F. R1_F is computed in
% the fit function, but we precompute the rest out here to save a
% few cpu cycles in the loop below.
W_F = (w1rms./(2*pi*delta)).^2/.055;
warning('off','all');
if(useParfor)
    parfor(ii=1:sz(2))
        % Some voxels produce a "Input to EIG must not contain NaN
        % or Inf" error in lsqnonl in. Tweaking the bounds or
        % starting estimate can fix it sometimes, but they are
        % probably junk voxels anyway, so we'll catch and skip them.
        try
            if(fitMethod=='l')
                [x, resnorm, residual, exitflag, output] = lsqnonlin(@(x) relaxMtFitFunc(x, data(:,ii), W_B, W_F, T2_B, r1(ii), s0(ii), t_m, t_s, t_r), x0, lb, ub, options); %bese j-12;
            else
                [x, resnorm, exitflag] = fminsearch(@(x) relaxMtFitFuncLs(x, data(:,ii), W_B, W_F, T2_B, r1(ii), s0(ii), t_m, t_s, t_r), x0, options);
            end
            %disp([ii x])
            if(exitflag>0)
                k(ii) = x(1);
                f(ii) = x(2);
                gof(ii) = resnorm;
            else
                gof(ii) = NaN;
            end
        catch
            % Leave the fit values at zero.
        end
    end
else
    for(ii=1:sz(2))
        try
            if(fitMethod=='l')
                [x, resnorm, residual, exitflag, output] = lsqnonlin(@(x) relaxMtFitFunc(x, data(:,ii), W_B, W_F, T2_B, r1(ii), s0(ii), t_m, t_s, t_r), x0, lb, ub, options); %bese j-12;
            else
                [x, resnorm, exitflag] = fminsearch(@(x) relaxMtFitFuncLs(x, data(:,ii), W_B, W_F, T2_B, r1(ii), s0(ii), t_m, t_s, t_r), x0, options);
            end
            if(exitflag>0)
                k(ii) = x(1);
                f(ii) = x(2);
                gof(ii) = resnorm;
            else
                gof(ii) = NaN;
            end
        catch
           % Leave the fit values at zero.
        end
    end
end
warning on;

return;


function a = c8mat_nint ( m, n, a )

%*****************************************************************************80
%
%% C8MAT_NINT rounds the entries of a C8MAT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the order of the array.
%
%    Input, complex A(M,N), the array to be rounded.
%
%    Output, complex A(M,N), the rounded array.
%
  for j = 1 : n
    for i = 1 : m
      a(i,j) = c8_nint ( a(i,j) );
    end
  end

  return
end


% SATGLOBE - Draw an idealized satellite view of earth
%
% This file renders a fully manipulatable satellite view
% of earth at a resolution of two pixels per degree, with added
% international political boundaries and gridlines.
% The imagery was obtained from NASA, and the globe was
% rendered using the Matlab Mapping Toolbox.
%
% The Mapping Toolbox is not needed to use this file.
%
% In order to save storage space, this m-file loads image
% data from the file satglobe.mat, and then creates the
% graticule mesh itself. This process allows users who
% do not have the Matlab Mapping Toolbox to render the
% figure, but it does take a few moments to compute the
% mesh. Using this trick, the data storage is reduced
% considerably; however, once the figure is redered, you
% may wish to save it as a regular Matlab figure file
% to increase speed.
%
% Michael Kleder, 2004

function satglobe
load satglobe
th=repmat((0:.5:180)'*pi/180,[1 721]);
ph=repmat((-180:.5:180)*pi/180,[361 1]);
s.children(1).properties.XData = sin(th).*cos(ph);
s.children(1).properties.YData = sin(th).*sin(ph);
s.children(1).properties.ZData = cos(th);
s.children(1).properties.CData = double(c)/255;
figure;
struct2handle(s,gcf);
set(gcf,'color','k','renderer','zbuffer','inverthardcopy','off','name',...
   'Earth at 2 Pixels per Degree, by Michael Kleder');
return


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


function [tout, info] = transfdiff(opt, tp, tm)
% TRANSFDIFF Transform diffusion algorithm for sequence of images.
%
% This function is part of a larger algorithm that solves the following
% registration problem:
%
% We have a sequence of images I=1,2,...,N. We want to register each image
% to its adjacent neighbours to form a volume. But instead of registering
% I=2 to I=1, and then I=3 to the result, and then I=4 to the result, etc.,
% it can be shown that all images can be aligned in parallel using an
% iterative process that we call "transform diffusion registration".
%
% TRANSFDIFF solves the "transform diffusion" step in the algorithm:
%
% Let's assume that somebody has already registered each image I to its
% neighbours I-1 and I+1. TRANSFDIFF takes that set of registration
% parameters and "diffuses" them to produce a transform for each image.
% Applying these transforms to the images produces the globally aligned
% sequence.
%
%
% TRANSFDIFF applies a diffusion process to those neighbour transforms. The
% output is the N transforms that applied to the N images best align them.
%
% [TOUT, INFO] = TRANSFDIFF(OPT, ...)
%
%   OPT is a string with the name of the transform to apply, or a struct
%   with the transform name and algorithm parameters. Options common to all
%   transformations:
%
%     'MaxIter': (def 50) Stopping criterion. The algorithm stops after
%                MaxIter diffusion iterations.
%
%     'Alpha':   (def 0.45) Diffusion coefficient. The smaller the value of
%                alpha, more iterations are needed for the same result. But
%                alpha >0.45 may not smooth high frequencies very well, and
%                values >=0.5 cause oscillations.
%
%   TOUT is an array with N output transforms for the N slices.
%
%   INFO is a struct with information about the algorithm internals:
%
%     'NumIter': Number of diffusion iterations until stop condition.
%
% -------------------------------------------------------------------------
%
% * OPT='TranslationTransform'
% * OPT.Transform='TranslationTransform'
%
% [TOUT, INFO] = TRANSFDIFF(OPT, TP)
%
%   TP is a matrix with N-1 rows. TP(I, :) is the translation from slice I
%   to slice I+1.
%
%   This case assumes that the transforms are symmetric, i.e. the
%   translation from slice I+1 to I is -TP(I, :).
%
%   Specific OPT options:
%
%     'Epsilon': (def 0.0) Stopping criterion. It stops when all
%                translation components are small:
%                for all t, abs(t)<=Epsilon.
%
% -------------------------------------------------------------------------
%
% * OPT='AffineTransform'
% * OPT.Transform='AffineTransform'
%
% * OPT='EulerTransform'
% * OPT.Transform='EulerTransform'
%
% [TOUT, INFO] = TRANSFDIFF(OPT, TP)
%
%   TP is an array of affine transforms in homogeneous coordinates. TP(I,:)
%   is the affine transform from slice I to slice I+1. Each transform has
%   the format
%
%   TP(:, :, I) = [A 0]
%                 [d 1]
%
%   where A is a matrix and d is a translation vector, and the affine
%   transform of a row vector X is defined as Y=X*TP(:,:,I).
%
%   This case assumes that the transforms are symmetric, i.e. the
%   transformation from slice I+1 to I is inv(TP(I, :)).
%
%   Specific OPT options:
%
%     'Epsilon': (def 0.0) Stopping criterion. It stops when the norm of
%                the difference between the transformation and the identity
%                matrix is small:
%                for all t, norm(t-eye(3))<=Epsilon.
%
% -------------------------------------------------------------------------
%
% * OPT.Transform='BsplineTransform'
%
% [TOUT, INFO] = TRANSFDIFF(OPT, TP, TM)
%
%   TP is a matrix with N-1 rows. Each row contains the B-spline
%   coefficients with the transformation from slice I to slice I+1. The
%   format of the B-spline coefficient vector is
%   [p0x, p1x, ..., pNx, p0y, p1y, ..., pNy].
%
%   TM is similar to TP, but TM(I, :) is the transformation from slice I+1
%   to I.
%
%   Specific OPT options:
%
%     'Epsilon': (def 0.0) Stopping criterion. It stops when all
%                coefficient displacement components are small:
%                for all t, abs(t)<=Epsilon.
%
%     'tbsp': (def []) For Choi and Lee (2000) injectivity criteria. These
%             are sufficient criteria that guarantee the B-spline doesn't
%             fold over. Transform struct in elastix format. The
%             tbsp.TransformParameters are ignored, only the grid
%             information is used. If not provided or empty, the conditions
%             are not used as a stopping criterion, and the resulting
%             B-splines may fold over.
%
%
% -------------------------------------------------------------------------
%
% See also: transdiffreg.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2015-2016 University of Oxford
% Version: 0.4.4
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% 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. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
switch (opt.Transform)

    case {'TranslationTransform', 'EulerTransform', 'AffineTransform'}

        narginchk(2, 2);

    case {'BSplineTransform'}

        narginchk(2, 3);

    otherwise

        error(['Transform ' opt.Transform ' not implemented'])

end
nargoutchk(0, 2);

% defaults
if (isempty(opt))
    error('OPT cannot be empty');
elseif (ischar(opt))
    aux = opt;
    clear opt
    opt.Transform = aux;
end
if (~isfield(opt, 'Epsilon'))
    opt.Epsilon = 0.0;
end
if (~isfield(opt, 'MaxIter'))
    opt.MaxIter = 50;
end
if (~isfield(opt, 'Alpha'))
    opt.Alpha = 0.45;
end
if (strcmp(opt.Transform, 'BSplineTransform'))
    if (~isfield(opt, 'tbsp'))
        opt.tbsp = [];
    end
end

if (opt.Alpha < 0 || opt.Alpha > 0.5)
    warning('alpha must be in the interval [0.0, 0.5]. alpha=0.5 can produce oscillations. We recommend alpha<=0.45')
end

% convert to array format if transforms are provided in elastix format
[tp, tp0] = elastix2mat(tp);
if (nargin > 2)
    [tm, tm0] = elastix2mat(tm);
end

% preprocessing of inputs
switch (opt.Transform)

    case {'TranslationTransform', 'BSplineTransform'}

        % compute "tm" from "tp"
        if (nargin < 3 || isempty(tm))
            tm = -tp;
        end

        % add dummy transforms at the beginning of "tm" and at the end of
        % "tp". This makes it easier to operate, because then:
        % * tp(I) and tm(I) are the neighbours of I for intermediate slices
        % * extreme slices can be dealt with in the same way as
        %   intermediate slices
        tp = tp([1:end end], :);
        tm = tm([1 1:end], :);
        tp(end, :) = tm(end, :);
        tm(1, :) = tp(1, :);

    case {'EulerTransform', 'AffineTransform'}

        % compute "tm" from "tp"
        if (nargin < 3 || isempty(tm))
            for I = 1:size(tp, 3)
                tm(:, :, I) = inv(tp(:, :, I));
            end
        end

        % add dummy transforms at the beginning of "tm" and at the end of
        % "tp". This makes it easier to operate (see note above)
        tp = tp(:, :, [1:end end]);
        tm = tm(:, :, [1 1:end]);
        tp(:, :, end) = tm(:, :, end);
        tm(:, :, 1) = tp(:, :, 1);

end

% check inputs
if any(size(tp) ~= size(tm))
    error('TP and TM must have the same size')
end

% if no transforms provided, then we don't need to run the algorithm
if (isempty(tp))
    tout = [];
    return
end

% apply registration diffusion
switch (opt.Transform)

    case {'TranslationTransform', 'BSplineTransform'}

        [tout, info] = translation_diffusion(opt, tp, tm);

    case {'EulerTransform', 'AffineTransform'}

        [tout, info] = affine_diffusion(opt, tp, tm);

    otherwise

        error('Transform not implemented')

end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% LOCAL FUNCTIONS

%% convert transform from elastix format to array, if necessary
function [tout, t0] = elastix2mat(t)

if (isstruct(t))

    % save copy of input so that we can convert the output back to elastix
    % format
    t0 = t;

    switch (t(1).Transform)

        case {'TranslationTransform', 'BSplineTransform'}

            tout = cat(1, t(:).TransformParameters);

        % TODO: AffineTransform

        otherwise

            error('Conversion from this elastix transform to Matlab array not implemented')
    end

else % already in array format

    % no need to save a copy of the input
    t0 = [];

    % output is just the input
    tout = t;

end

end

%% translation_diffusion: Apply diffusion to translation transforms
function [tout, info] = translation_diffusion(opt, tp, tm)

% init accumulated transform to apply to each slice
tout = zeros(size(tp));

I = 0;
while (1)

    % iteration number
    I = I + 1;

    % transform to apply to each slice in this iteration (transform update)
    t = opt.Alpha * (tp + tm);

    % stopping criterion: B-spline injectivity
    if (strcmp(opt.Transform, 'BSplineTransform'))

        % find control points that don't guarantee injectivity and
        % constrain them so that they do
        [info.NConstrained(I), aux] = ...
            check_bspline_injectivity_choi2000(tout + t, opt.tbsp);

        % apply the control point modifications to the B-spline update
        t = aux - tout;

    end

    % compose with the previous transforms to obtain a total transform
    tout = tout + t;

    % update neighbour transforms ...

    % ... of internal images
    tp(1:end-1, :) = t(2:end, :) + tp(1:end-1, :) - t(1:end-1, :);
    tm(2:end, :) = t(1:end-1, :) + tm(2:end, :) - t(2:end, :);

    %... of extreme images
    tp(end, :) = tm(end, :);
    tm(1, :) = tp(1, :);

    % stopping criterion: absolute value of each translation component
    tabs = abs(t(:));
    info.MaxAbs(I) = max(tabs);
    info.MeanAbs(I) = mean(tabs);
    info.MedAbs(I) = median(tabs);
    if (info.MaxAbs(I) <= opt.Epsilon)
        break
    end

    % stopping criterion: maximum number of iterations
    if (I == opt.MaxIter)
        break
    end

end

info.NumIter = I;

% DEBUG:
% isInjective = check_bspline_injectivity_global(tout + t, opt.tbsp);
% nnz(~isInjective)

end

%% affine_diffusion: Apply diffusion to affine transforms
function [tout, info] = affine_diffusion(opt, tp, tm)

% init accumulated transform to apply to each slice
tout = eye(size(tp(:, :, 1)));
tout = repmat(tout, 1, 1, size(tp, 3));

% allocate memory for transform update
t = zeros(size(tp));

I = 0;
while (1)

    % iteration number
    I = I + 1;

    % compute the transform to apply to each slice in this iteration
    for J = 1:size(tp, 3)

        % transform to apply to each slice in this iteration (transform
        % update)
        t(:, :, J) = real(expm(opt.Alpha ...
            * (logm(tp(:, :, J)) + logm(tm(:, :, J)))));

        % compose with the previous transforms to obtain a total transform
        tout(:, :, J) = tout(:, :, J) * t(:, :, J);

    end

    % update neighbour transforms of internal images
    for J = 2:size(tm, 3)
        tm(:, :, J) = t(:, :, J) \ tm(:, :, J) * t(:, :, J-1);
    end
    for J = 1:size(tp, 3)-1
        tp(:, :, J) = t(:, :, J) \ tp(:, :, J) * t(:, :, J+1);
    end

    % update neighbour transforms of extreme images
    tp(:, :, end) = tm(:, :, end);
    tm(:, :, 1) = tp(:, :, 1);

    % stopping criteria...
    % ... norm of the transform update different from identity
    tnorm = zeros(1, size(t, 3));
    for J = 1:size(t, 3)
        tnorm(J) = norm(t(:, :, J) - eye(size(t(:, :, 1))), 2);
    end
    info.MaxTNorm(I) = max(tnorm);
    info.MeanTNorm(I) = mean(tnorm);
    info.MedTNorm(I) = median(tnorm);
    if (all(tnorm <= opt.Epsilon))
        break
    end

    % ... maximum number of iterations
    if (I == opt.MaxIter)
        break
    end

end

info.NumIter = I;

end

%% check_bspline_injectivity_global:
%
% Check B-spline global injectivity triangulating the grid and looking for
% flipped triangles.
%
% tout: (N, K)-matrix, N number of slices, K number of B-spline control
%       points
% tbsp: elastix struct with the B-spline info (we need the grid spacing)
function isInjective = check_bspline_injectivity_global(tout, tbsp)

DEBUG = false;

% number of slices
N = size(tout, 1);

% grid of control points for 0 deformation
tbsp.TransformParameters(:) = 0;
[gx0, gy0] = elastix_bspline_grid(tbsp);
tri = delaunay(gx0(:), gy0(:));

% area of the triangles
atri = trifacet_signed_area(tri, [gx0(:), gy0(:)]);

% flip triangles with negative area, so that all are positive
idx = atri < 0;
tri(idx, :) = tri(idx, end:-1:1);

% DEBUG: plot control points
if (DEBUG)
    trimesh(tri, gx0(:), gy0(:))
end

isInjective = true(1, N);
for J = 1:N

    % transfer coefficients to the B-spline
    tbsp.TransformParameters = tout(J, :);

    % apply transformation to the control points (note:
    aux = transformix_pts(tbsp, [gx0(:) gy0(:)]);
    gx1 = reshape(aux(:, 1), size(gx0, 1), size(gx0, 2));
    gy1 = reshape(aux(:, 2), size(gy0, 1), size(gy0, 2));

    % DEBUG: plot control points
    if (DEBUG)
        trimesh(tri, gx1(:), gy1(:))
    end

    % area of the triangles
    atri = trifacet_signed_area(tri, [gx1(:), gy1(:)]);

    % if any triangle is flipped, the B-spline is not injective
    isInjective(J) = all(atri > 0);

end

end

%% check_bspline_injectivity_choi2000:
%
% Check B-spline injectivity using sufficient conditions in Choi and Lee
% (2000). Constrain control points to guarantee injectivity.
%
% This function checks Choi and Lee (2000) conditions for injectivity of
% the B-spline. The conditions are sufficient, i.e. if any is fulfilled,
% then we know the B-spline is injective. If not, the B-spline may or may
% not be injective, but we'll assume it's not.
%
% tout: (N, K)-matrix, N number of slices, K number of B-spline control
%       points
% tbsp: elastix struct with the B-spline info (we need the grid spacing)
%
% nConstrained: number of control points that had to be constrained to
%               guarantee injectivity
function [nConstrained, tout] = check_bspline_injectivity_choi2000(tout, tbsp)

% constants from Choi and Lee (2000), to check violations
K2 = 2.046392675;
A2 = sqrt((3/2)^2 + (K2 - 3/2)^2);

% number of slices
N = size(tout, 1);

% Note: the grid has row->x, col->y coordinates, instead of the Matlab
% convention, which is the opposite

% number of B-spline control points
L = length(tbsp.TransformParameters)/2;
Nx = tbsp.GridSize(1);
Ny = tbsp.GridSize(2);

% loop slices
isInjective = false(Nx, Ny, N);
for J = 1:N

    %% find control points that are guaranteed to not cause injectivity
    %% problems

    % transfer coefficients to the B-spline
    tbsp.TransformParameters = tout(J, :);

    % easy nomenclature for x, y coordinates of control points
    cx = tbsp.TransformParameters(1:L);
    cy = tbsp.TransformParameters(L+1:end);

    % reshape coefficients into grid
    cx = reshape(cx, Nx, Ny);
    cy = reshape(cy, Nx, Ny);

    % normalize control point displacement to grid spacing = 1
    cx = cx / tbsp.GridSpacing(1);
    cy = cy / tbsp.GridSpacing(2);

    % Theorem 1 from Choi and Lee (2000): first sufficient condition
    % for injectivity
    cond1 = (abs(cx) < 1 / K2) & (abs(cy) < (1 / K2));

    % Theorem 2 from Choi and Lee (2000): second sufficient condition
    % for injectivity
    cond2 = cx.^2 + cy.^2 < (1 / A2)^2;

    % if either condition is fulfilled, then the B-spline is injective
    isInjective(:, :, J) = cond1 | cond2;

    %% correct control points that potential can cause non-injectivity

    % find problematic points (linear index easier than row/column
    % index)
    idx = find(~isInjective(:, :, J));

    % correction factor for the problematic coefficients
    S = 1 ./ sqrt(cx(idx).^2 + cy(idx).^2) / (A2 - 0.01);

    % correct problematic control points
    cx(idx) = cx(idx) .* S;
    cy(idx) = cy(idx) .* S;

    % transfer corrected coefficients to the output
    aux = tout(J, :);
    aux(idx) = tbsp.GridSpacing(1)*cx(idx);
    aux(L + idx) = tbsp.GridSpacing(2)*cy(idx);
    tout(J, :) = aux;

end

% count number of control points that had to be constrained to guarantee
% they are injective
nConstrained = nnz(~isInjective);

end

%% check_bspline_injectivity_chun2009:
%
% Check B-spline injectivity using sufficient conditions in Chun and
% Fessler (2009).
%
% This function checks Chun and Fessler (2009) conditions for injectivity
% of the B-spline. If all the conditions are fulfilled, then we know the
% B-spline is injective. If not, the B-spline may or may not be injective,
% but we'll assume it's not.
%
% Because the conditions check grid edge lengths, it's not trivial to
% associate the conditions to vertices.
%
% tout: (N, K)-matrix, N number of slices, K number of B-spline control
%       points
% tbsp: elastix struct with the B-spline info (we need the grid spacing)
function isInjective = check_bspline_injectivity_chun2009(tout, tbsp)

DEBUG = false;

% constants from Chun and Fessler (2009), to check for violations
kx = 0.5 - 0.01;
ky = 0.5 - 0.01;

% number of slices
N = size(tout, 1);

% number of B-spline coefficients
L = length(tbsp.TransformParameters);
Nx = tbsp.GridSize(1);
Ny = tbsp.GridSize(2);

% DEBUG: plot control points
if (DEBUG)
    % grid of control points for 0 deformation
    tbsp.TransformParameters(:) = 0;
    [gx0, gy0] = elastix_bspline_grid(tbsp);
    tri = delaunay(gx0(:), gy0(:));

    trimesh(tri, gx0(:), gy0(:))
end

% loop slices
isInjective = true;
for J = 1:N

    % transfer coefficients to the B-spline
    tbsp.TransformParameters = tout(J, :);

    % easy nomenclature for x, y coordinates of control points
    cx = tbsp.TransformParameters(1:L/2);
    cy = tbsp.TransformParameters(L/2+1:end);

    % reshape coefficients into grid
    cx = reshape(cx, Nx, Ny);
    cy = reshape(cy, Nx, Ny);

    % transpose grid so that we have x->cols, y->rows
    cx = cx';
    cy = cy';

    % grid spacing
    mx = tbsp.GridSpacing(1);
    my = tbsp.GridSpacing(2);

    % conditions (i->x, j->y):

    % -mx * kx <= c_{i+1,j}^x - c_{i,j}^x
    % we also add a column to the right to account for the lost column
    cond = (diff(cx, 1, 2) >= -mx * kx);
    isInjective = isInjective && all(cond(:));

    % -my * ky <= c_{i,j+1}^y - c_{i,j}^y
    % we also add a row to the bottom to account for the lost row
    cond = (diff(cy, 1, 1) >= -my * ky);
    isInjective = isInjective && all(cond(:));

    % |c_{i,j+1}^x - c_{i,j}^x| <= mx * kx
    % we also add a row to the bottom to account for the lost row
    cond = (abs(diff(cx, 1, 1)) <= mx * kx);
    isInjective = isInjective && all(cond(:));

    % |c_{i+1,j}^y - c_{i,j}^y| <= my * ky
    % we also add a column to the right to account for the lost column
    cond = (abs(diff(cy, 1, 2)) <= my * ky);
    isInjective = isInjective && all(cond(:));

end

end


function [r,xp] = spm_dcm_erp_bma (BMS_file,stats,params)
% Compute posterior over connections, or modulatory gains in them, from BMA
% FORMAT [r,xp] = spm_dcm_erp_bma (BMS_file,stats,params)
%
% BMS_file      Name of Bayesian Model Selection .mat file
% stats         'ffx' or 'rfx'
% params        Parameter data structure
%               .type          'A' (connection) or 'B' (gain in connection)
%               .hier          'forward', 'backward' or 'lateral' (for conn_type='A')
%               .ip            eg 1, 2, 3 indexes modulatory input (for conn_type='B')
%               .to            to region eg 3
%               .from          from region eg 1
%               .xt            exceedance threshold (typically set to 1)
%               .C             [nr x nr] contrast matrix where nr is the number of regions
%
%
% r             posterior samples
% xp            exceedance probability
%               This is the posterior probability that the connection is
%               larger than params.xt. Alternatively, if you are looking at
%               a contrast of connections, its the posterior probability
%               that the contrast is greater than zero.
%
% The parameters returned by Bayesian Model Averaging (BMA) are the 'latent'
% variables A and B which are Gaussian (and consequently can be positive or
% negative).
%
% The corresponding connection strengths (rA) or gains in connection
% strength (rB) are an exponential function of these latent variables.
% These are the values we are interested in and want to make an inference
% about.
%
% This routine computes the posterior distribution over rA or rB by
% generating samples from the latent variables, and exponentiating each
% sample.
%
% The probability that the rA or RB values are greater than some threshold
% xt (such as unity) is then just the proportion of posterior samples that
% are greater than xt.
%
% If a contrast matrix (C) is not specifed this function looks at a single
% connection or gain. To look at relative sizes of connection/gain values
% enter a C matrix. eg. to test, in a 3 region DCM, is connection from 3
% to 2 bigger than 2 to 3 ? set C=[0 0 0; 0 0 1; 0 -1 0].
%
%--------------------------------------------------------------------------
%
% Example usage:
%
% 1. Look at a single connection value:
%
% params.type='A'; params.hier='forward';
% params.to=3; params.from=1; params.xt=1;
% spm_dcm_erp_bma([],'ffx',params);
%
% 2. Look at a single gain value:
%
% params.type='B'; params.ip=1;
% params.to=1; params.from=1; params.xt=1;
% spm_dcm_erp_bma([],'ffx',params);
%
% 3. Look at a contrast of connection values:
%
% params.type='B'; params.ip=1;
% params.C=[0 0 0; 0 0 1; 0 -1 0];
% spm_dcm_erp_bma([],'ffx',params);
%__________________________________________________________________________
% Copyright (C) 2011 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_dcm_erp_bma.m 4529 2011-10-18 14:58:20Z guillaume $

%-Get parameters
%--------------------------------------------------------------------------
try
    load (BMS_file);
catch
    [BMS_file,sts] = spm_select(1,'^BMS\.mat$','Select BMS.mat file');
    if ~sts, return; end
    load (BMS_file);
end

if isfield(params,'C')
    con = 1;
else
    con = 0;
end

switch lower(stats)
    case 'ffx'
        bma = BMS.DCM.ffx.bma;
    case 'rfx'
        bma = BMS.DCM.rfx.bma;
    otherwise
        error('The stats field shout be set to ''ffx'' or ''rfx''');
end

%-Get mean and SD of effects
%--------------------------------------------------------------------------
switch params.type
    case 'A'
        hier_type = {'forward','backward','lateral'};
        h = find(ismember(hier_type,params.hier));
        M = bma.mEp.A{h};
        S = bma.sEp.A{h};
        if ~con
            str=sprintf('%s connection (rA) from region %d to %d:',hier_type{h},params.from,params.to);
        end
    case 'B'
        M = bma.mEp.B{params.ip};
        S = bma.sEp.B{params.ip};
        if ~con
            str=sprintf('Effect (rB) of modulatory input %d on connection from region %d to %d:',params.ip,params.from,params.to);
        end
    otherwise
        error('conn_type must be ''A'' or ''B''');
end

%-Generate N samples in latent space, then exponentiate to get estimate
% of effects
%--------------------------------------------------------------------------
N=10000;

switch con
    case 1
        % Look at contrast of connections
        nr = size(M,1);
        m  = M(:);
        s  = S(:);
        c  = params.C(:);

        % Full posterior covariances are not stored
        % only univariate SD's so use these
        x  = (s*ones(1,N)).*randn(nr*nr,N)+m*ones(1,N);
        r  = c'*exp(x);
        rm = mean(r);
        xp = length(find(r>0))/length(r);
        switch params.type
            case 'A'
                str=sprintf('Contrast of %s connections (rA)',hier_type{h});
            case 'B'
                str=sprintf('Contrast of effects (rB) of modulatory input %d ',params.ip);
        end
    case 0
        % Just look at a single connection
        m  = M(params.to,params.from);
        s  = S(params.to,params.from);
        x  = s*randn(N,1)+m;
        r  = exp(x);
        rm = mean(r);
        xp = length(find(r>params.xt))/length(r);
end

%-Display
%--------------------------------------------------------------------------
figure
hist(r,20);
set(gca,'FontSize',18);
set(gca,'YTickLabel',[]);
ylabel(sprintf('p(r%s|Y)',params.type));
xlabel(sprintf('r%s',params.type));

%-Report
%--------------------------------------------------------------------------
fprintf('%s\n',str);
fprintf('Posterior mean = %1.2f\n',rm);
fprintf('Exceedance probability = %1.6f\n', xp);


function y = callback_dct_wavelets(x, dir, options)

% callback_dct_wavelets - callback for union of DCT and Wavelets dictionary
%
%   y = callback_dct_wavelets(x, dir, options);
%
%   Copyright (c) 2008 Gabriel Peyre

options.remove_lowfreq = 0;
if not(isfield(options, 'dct_type'))
    options.dct_type = 'redundant';
end
if dir==+1
    y = callback_localdct(x{1}, dir, options) + callback_atrou({x{2:end}}, dir, options);
%    y = y/2;
elseif dir==-1
    yd = callback_localdct(x,dir,options);
    yw = callback_atrou(x,dir,options);
    y = { yd, yw{:}};
else
    error('Pseudo inverse not implemented');
end


function [DataOut, Metric] = ReceiverHD(HDchips, G, Scrambler);
%
% ReceiverHD    Hard Decision Receiver. This function performs transmitted packet data recovering,
%                          based on Viterbi Decoder.
%
% 						Block Diagram:
%						HDchips -> [DeScrambler] -> [De-Interleaver] -> [Viterbi Decoder]
%
%
% 						Inputs: HDchips - Hard Decision RAKE receiver symbols
%                         G - Viterbi Encoder generation polynom
%                         Scrambler - Scrambler sequence
%
% 						Outputs: DataOut - Received data (binary form)
%                                       Metric - The  best metric of Viterbi Decoder
%

%====================== COMMON DEFINITIONS =================
%------- Viterbi Polynom ---------
if (nargin == 1)
   G = [1 1 1 1 0 1 0 1 1; 1 0 1 1 1 0 0 0 1];
end

%============================== R E C E I V E R ==========================

%----------- DeScrambler---------
% Rate = 19.2 KBps
HDchips = xor(HDchips, Scrambler);

%-------- DeInterleaver ---------
INTERL = reshape(HDchips, 16, 24);		% IN-> rows, OUT-> columns
HDchips = reshape(INTERL', length(HDchips), 1);  % Rate = 19.2 KBps

%-------- HD Input Viterbi Decoder ----------
[DataOut Metric] = VitDec(G, HDchips, 1);


function y = idct2(x)
  t = idct(x);
  t = idct(t');
  y = t';