function varargout = linearProgramming(maximizeFlag, f, A, b, lb, ub, constraintTypes, variableTypes, options)
% Provides a common interface to several linear programming solvers.
% Checks each solver for errors.
%
% That is it solves the problem:
%   max f'*x
%   subject to A*x~b, lb<=x<=ub
% where "~" can be equality, or double or single-sided inequality
% constraints if the chosen linear programming solver supports those
% types of constraints. Additionally the variables, x, can, if the chosen
% linear programming solver supports it, either continuous, integer, and
% boolean valued.
%
% Constraint Types: etiher a single value, or a column vector or values:
%   'F' Free (unbounded) variable (the constraint is ignored).
%   'U' Variable with upper bound ( A(i,:)*x <= b(i)).
%   'S' Fixed Variable (A(i,:)*x = b(i)).
%   'L' Variable with lower bound (A(i,:)*x >= b(i)).
%   'D' Double-bounded variable (A(i,:)*x >= -b(i) and A(i,:)*x <= b(i)).
%
% Variable Types: either a single value, or a column vector or values:
%   'C' Continuous variable.
%   'I' Integer variable
%   'B' Binary variable
%
% Output
%  x
%  lambda,
%  fopt
%  errorFlag
%  errorMsg
%  extra
%
% Requirements: at least one of the following linear programing solvers
% - glpk -- GNU Linear programming kit, to solve linear programming problems
%   http://sourceforge.net/projects/glpkmex/
% - lp_solve -- open source solve linear programming solver
%   http://sourceforge.net/projects/lpsolve
% - clp -- open source solve linear programming solver
%   http://control.ee.ethz.ch/~joloef/mexclp.zip
% - bpmpd -- open source solve linear programming solver
%   http://www.pserc.cornell.edu/bpmpd/
% - qsopt -- open source solve linear programming solver
%   http://control.ee.ethz.ch/~joloef/mexqsopt.msql
% - Optimization Toolbox - has linprog, MATLAB's linear programming
%   solver. Note: linprog is not vevy good. We suggest you use another
%   solver instead.
%   http://www.mathworks.com/products/optimization/
%
% Author: Jonathan Karr
% Affiliation: Covert Lab, Department of Bioengineering, Stanford University
% Last updated: 10/31/2008

%- A is non-empty
%- b is real column vector of length ncols(A)
%- lb, ub are real column vectors of length nrows(A)
validateattributes(A, {'numeric'}, {'nonempty'});
validateattributes(b, {'numeric'}, {'real', 'size', [size(A, 1) 1]});
if isempty(lb)
    lb = -Inf(size(A, 2), 1);
else
    validateattributes(lb, {'numeric'}, {'real', 'size', [size(A, 2) 1]});
end
if isempty(ub)
    ub = -Inf(size(A, 2), 1);
else
    validateattributes(ub, {'numeric'}, {'real', 'size', [size(A, 2) 1]});
end

%error check length sizes of contraint and variable types, expand if
%necessary
if isscalar(constraintTypes)
    if ~ismembc(constraintTypes, 'DFLSUdflsu')
        throw(MException('ComputationUtil:LinearProgramming', 'linear programming: invalid constraints types'));
    end
    constraintTypes = constraintTypes(ones(length(b), 1), 1);
elseif length(constraintTypes) ~= length(b)
    throw(MException('ComputationUtil:LinearProgramming', 'linear programming: invalid constraints types'));
elseif ~all(ismembc(constraintTypes, 'DFLSUdflsu'))
    throw(MException('ComputationUtil:LinearProgramming', 'linear programming: invalid constraints types'));
end

if isscalar(variableTypes)
    if ~ismembc(variableTypes, 'BCIbci')
        throw(MException('ComputationUtil:LinearProgramming', 'linear programming: invalid variable types'));
    end
    variableTypes = variableTypes(ones(size(A, 2), 1), 1);
elseif length(variableTypes) ~= length(lb)
    throw(MException('ComputationUtil:LinearProgramming', 'linear programming: invalid variable types'));
elseif ~all(ismembc(variableTypes, 'BCIbci'))
    throw(MException('ComputationUtil:LinearProgramming', 'linear programming: invalid variable types'));
end

%compute sense from maximizeFlag
%if maximize='maximize'->sense=-1
%otherwise->sense=1
sense = 1 - 2 *strcmp(maximizeFlag, 'maximize');

%solver linear programming problem using chosen solver
varargout = cell(max(nargout, 1), 1);
switch options.solver(1:2)
    case 'gl'; [varargout{:}] = runglpk(   f, A, b, lb, ub, sense, constraintTypes, variableTypes, options);
    case 'li'; [varargout{:}] = runlinprog(f, A, b, lb, ub, sense, constraintTypes, variableTypes, options);
    case 'bp'; [varargout{:}] = runbpmpd(  f, A, b, lb, ub, sense, constraintTypes, variableTypes, options);
    case 'cl'; [varargout{:}] = runclp(    f, A, b, lb, ub, sense, constraintTypes, variableTypes, options);
    case 'lp'; [varargout{:}] = runlpsolve(f, A, b, lb, ub, sense, constraintTypes, variableTypes, options);
    case 'qs'; [varargout{:}] = runqsopt(  f, A, b, lb, ub, sense, constraintTypes, variableTypes, options);
    case 'to'; [varargout{:}] = runtomlab( f, A, b, lb, ub, sense, constraintTypes, variableTypes, options);
    case 'cp'; [varargout{:}] = runcplex(  f, A, b, lb, ub, sense, constraintTypes, variableTypes, options);
    case 'gu'; [varargout{:}] = rungurobi( f, A, b, lb, ub, sense, constraintTypes, variableTypes, options);
    case 'mo'; [varargout{:}] = runmosek(  f, A, b, lb, ub, sense, constraintTypes, variableTypes, options);
    otherwise; throw(MException('ComputationUtil:LinearProgramming', 'Support for %s not implemented', options.solver));
end

function [x, lambda, fopt, errorFlag, errorMsg, extra] = runlinprog(f, A, b, lb, ub, sense, constraintTypes, variableTypes, options)

%setup equality, inequality constraints based on constraint types
Aeq = [];
beq = [];
if sum(constraintTypes == 'S') == length(constraintTypes)
    Aeq = A;
    A = [];
    beq = b;
    b = [];
elseif sum(constraintTypes == 'U') == length(constraintTypes)
elseif sum(constraintTypes == 'L') == length(constraintTypes)
    A = -A;
    b = -b;
else
    throw(MException('ComputationUtil:LinearProgramming', 'linprog: constraint types invalid'));
end

%throw error if integer or boolean variable types requested, linprog
%doesn't have this capability
if sum(variableTypes == 'C') ~= length(variableTypes)
    throw(MException('ComputationUtil:LinearProgramming', 'linprog: variable types invalid'));
end

%linprog options
if isfield(options.solverOptions, 'linprog')
    linprogoptions = options.solverOptions.linprog;
else
    linprogoptions = struct;
end

try
    [x, fopt, exitflag, ~, lambda] = linprog(sense * f, A, b, Aeq, beq, lb, ub, [], linprogoptions);

    lambda = struct('reducedCosts', NaN(size(A, 2), 1), 'shadowPrices', lambda);
    fopt = fopt * sense;
    errorFlag = exitflag ~= 1;
    switch exitflag
        case  1; errorMsg = 'Function converged to a solution x.';
        case  0; errorMsg = 'Number of iterations exceeded options.MaxIter.';
        case -2; errorMsg = 'No feasible point was found.';
        case -3; errorMsg = 'Problem is unbounded.';
        case -4; errorMsg = 'NaN value was encountered during execution of the algorithm.';
        case -5; errorMsg = 'Both primal and dual problems are infeasible.';
        case -7; errorMsg = 'Search direction became too small. No further progress could be made.';
    end
catch exception
    errorFlag = 1;
    errorMsg = exception.message;
    x = NaN(size(lb));
    lambda = struct('reducedCosts', NaN(size(A, 2), 1), 'shadowPrices', NaN(size(A, 1), 1));
    fopt = NaN;
end

extra = struct;

function [x, lambda, fopt, errorFlag, errorMsg, extra] = runglpk(f, A, b, lb, ub, sense, constraintTypes, variableTypes, options)

%make structure of glpk options
if isfield(options.solverOptions, 'glpk')
    glpkoptions = options.solverOptions.glpk;
else
    glpkoptions = struct;
end

%call glpk
try
    [x, fopt, status, extra] = glpkcc(f, A, b, lb, ub, constraintTypes, variableTypes, sense, glpkoptions);
    lambda = struct('reducedCosts', extra.redcosts, 'shadowPrices', extra.lambda);
    errorFlag = ~(status == 5 || status == 2);
    switch status
        %General Errors
        case 5; errorMsg = 'Solution is optimal';
        case 2; errorMsg = 'Solution is feasible';
        case 1; errorMsg = 'Solution is undefined';
        case 3; errorMsg = 'Solution is infeasible';
        case 4; errorMsg = 'No feasible solution exists';
        case 6; errorMsg = 'Solution is unbounded';

        %Simplex method Errors:
        case 101; errorMsg = 'Invalid basis';
        case 102; errorMsg = 'Singular matrix';
        case 103; errorMsg = 'Ill-conditioned matrix';
        case 104; errorMsg = 'Invalid bounds';
        case 105; errorMsg = 'Solver failed';
        case 106; errorMsg = 'Objective lower limit reached';
        case 107; errorMsg = 'Objective upper limit reached';
        case 108; errorMsg = 'Iteration limit exceeded';
        case 109; errorMsg = 'Time limit exceeded';
        case 110; errorMsg = 'No primal feasible solution';

        %Interior point method, mixed integer problem Errors:
        case 204; errorMsg = 'Unable to start the search.';
        case 205; errorMsg = 'Objective function lower limit reached.';
        case 206; errorMsg = 'Objective function upper limit reached.';
        case 207; errorMsg = 'Iterations limit exhausted.';
        case 208; errorMsg = 'Time limit exhausted.';
        case 209; errorMsg = 'No feasible solution.';
        case 210; errorMsg = 'Numerical instability.';
        case 211; errorMsg = 'Problems with basis matrix.';
        case 212; errorMsg = 'No convergence (interior).';
        case 213; errorMsg = 'No primal feasible solution (LP presolver).';
        case 214; errorMsg = 'No dual feasible solution (LP presolver).';

        otherwise, errorMsg = sprintf('Invalid error code %d', status);
    end
catch exception
    errorFlag = 1;
    errorMsg = exception.message;
    x = NaN(size(lb));
    lambda = struct('reducedCosts', NaN(size(A, 2), 1), 'shadowPrices', NaN(size(A, 1), 1));
    fopt = NaN;
end

extra = struct;

function [x, lambda, fopt, errorFlag, errorMsg, extra] = runlpsolve(f, A, b, lb, ub, sense, constraintTypes, variableTypes, options)
%options
verbose = 3;
presolve = 0; %none
scaling = 4 + 64 + 128; %geometric + equilibrate + integers
if isfield(options.solverOptions, 'lp_solve')
    if isfield(options.solverOptions.lp_solve, 'verbose'), verbose = options.solverOptions.lp_solve.verbose; end
    if isfield(options.solverOptions.lp_solve, 'presolve'), presolve = options.solverOptions.lp_solve.presolve; end
    if isfield(options.solverOptions.lp_solve, 'scaling'), scaling = options.solverOptions.lp_solve.scaling; end
end

%convert variable types to lp_solve format'
if any(variableTypes == 'B'); throw(MException('ComputationUtil:LinearProgramming', 'lp_solve: boolean variables are not allowed')); end;
xint = NaN(size(variableTypes));
xint(variableTypes == 'I') = 1;
xint(variableTypes == 'C') = 0;

%convert constraints types to lp_solve format
if any(constraintTypes == 'F'); throw(MException('ComputationUtil:LinearProgramming', 'lp_solve: free constraints are not allowed')); end;
if any(constraintTypes == 'D'); throw(MException('ComputationUtil:LinearProgramming', 'lp_solve: double-sided constraints are not allowed')); end;
con_types = NaN(size(constraintTypes));
con_types(constraintTypes == 'L') = 2;
con_types(constraintTypes == 'S') = 3;
con_types(constraintTypes == 'U') = 1;

try
    lp = mxlpsolve('make_lp', size(A, 1), size(A, 2));
    mxlpsolve('set_verbose', lp, verbose);
    mxlpsolve('set_mat', lp, A);
    mxlpsolve('set_rh_vec', lp, b);
    mxlpsolve('set_obj_fn', lp, f);
    mxlpsolve('set_sense', lp, (sense == -1) + 0);
    mxlpsolve('set_constr_type', lp, con_types);
    mxlpsolve('set_bounds', lp, lb, ub);
    mxlpsolve('set_int', lp, xint);
    mxlpsolve('set_presolve', lp, presolve, mxlpsolve('get_presolveloops', lp));
    mxlpsolve('set_scaling', lp, scaling);

    status = mxlpsolve('solve', lp);
    errorFlag =~ status == 0 || status == 1 || status == 11 || status == 12;
    if ~errorFlag
        [fopt, x, duals] = mxlpsolve('get_solution', lp);
        reducedCosts = mxlpsolve('get_reduced_costs', lp);
    else
        fopt = NaN;
        x = NaN(size(lb));
        duals = NaN(size(A, 1), 1);
        reducedCosts = NaN(size(A, 2), 1);
    end

    extra = struct;
    if nargin >= 6
        tmpFileName = ['lpsolve.' datestr(now, 30) '.txt'];

        mxlpsolve('set_outputfile', lp, tmpFileName);
        mxlpsolve('print_scales', lp);

        fid = fopen(tmpFileName, 'r');
        fgetl(fid);
        fgetl(fid);
        line = fgetl(fid);
        extra.objScaling = str2double(line(32:end));
        extra.rowScaling = zeros(size(A, 1), 1);
        extra.colScaling = zeros(size(A, 2), 1);
        for i = 1:size(A, 1)
            line = fgetl(fid);
            extra.rowScaling(i) = str2double(line(32:end));
        end
        for i = 1:size(A, 2)
            line = fgetl(fid);
            extra.colScaling(i) = str2double(line(32:end));
        end
        fclose(fid);
    end

    mxlpsolve('delete_lp', lp);

    if nargin >= 6
        delete(tmpFileName);
    end

    lambda = struct('reducedCosts', reducedCosts, 'shadowPrices', duals);
    switch status
        case -2; errorMsg = 'Out of memory';
        case 0; errorMsg = 'An optimal solution was obtained';
        case 1; errorMsg = ['The model is sub-optimal. Only happens if there are integer variables and there is already an integer solution found. The solution is not guaranteed the most optimal one.\n'...
                '* A timeout occured (set via set_timeout or with the -timeout option in lp_solve)\n'...
                '* set_break_at_first was called so that the first found integer solution is found (-f option in lp_solve)\n'...
                '* set_break_at_value was called so that when integer solution is found that is better than the specified value that it stops (-o option in lp_solve)\n'...
                '* set_mip_gap was called (-g/-ga/-gr options in lp_solve) to specify a MIP gap\n'...
                '* An abort function is installed (put_abortfunc) and this function returned TRUE\n'...
                '* At some point not enough memory could not be allocated'];
        case 2; errorMsg = 'The model is infeasible';
        case 3; errorMsg = 'The model is unbounded';
        case 4; errorMsg = 'The model is degenerative';
        case 5; errorMsg = 'Numerical failure encountered';
        case 6; errorMsg = 'The abort routine returned TRUE. See put_abortfunc';
        case 7; errorMsg = 'A timeout occurred. A timeout was set via set_timeout';
        case 9; errorMsg = 'The model could be solved by presolve. This can only happen if presolve is active via set_presolve';
        case 10; errorMsg = 'The B&B routine failed';
        case 11; errorMsg = 'The B&B was stopped because of a break-at-first (see set_break_at_first) or a break-at-value (see set_break_at_value)';
        case 12; errorMsg = 'A feasible B&B solution was found';
        case 13; errorMsg = 'No feasible B&B solution found';
    end
catch exception
    errorFlag = 1;
    errorMsg = exception.message;
    x = NaN(size(lb));
    lambda = struct('reducedCosts', NaN(size(A, 2), 1), 'shadowPrices', NaN(size(A, 1), 1));
    fopt = NaN;
    extra = struct;
end

function [x, lambda, fopt, errorFlag, errorMsg, extra] = runqsopt(f, A, b, lb, ub, sense, constraintTypes, variableTypes, options)

%setup equality, inequality constraints based on constraint types
Aeq = [];
beq = [];
if sum(constraintTypes == 'S') == length(constraintTypes)
    Aeq = A;
    A = [];
    beq = b;
    b = [];
elseif sum(constraintTypes == 'U') == length(constraintTypes)
elseif sum(constraintTypes == 'L') == length(constraintTypes)
    A = -A;
    b = -b;
else
    throw(MException('ComputationUtil:LinearProgramming', 'qsopt: constraint types invalid'));
end

%throw error if integer or boolean variable types requested, qsopt
%doesn't have this capability
if sum(variableTypes == 'C') ~= length(variableTypes)
    throw(MException('ComputationUtil:LinearProgramming', 'qsopt: variable types invalid'));
end

%make structure of qsopt options
qsoptoptions = struct;
if isfield(options.solverOptions, 'qsopt')
    qsoptoptions = options.solverOptions.qsopt;
end

try
    [x, dual, status] = qsopt(sense * f, A, b, Aeq, beq, lb, ub, qsoptoptions);
    lambda = struct('reducedCosts', NaN(size(A, 2), 1), 'shadowPrices', dual);
    fopt = f' * x;
    errorFlag = status ~= 1;
    switch status
        case 1; errorMsg = 'optimal';
        case 2; errorMsg = 'infeasible';
        case 3; errorMsg = 'unbounded';
        case 4; errorMsg = 'iteration limit';
        case 5; errorMsg = 'time limit';
        case 6; errorMsg = 'other problem';
    end
catch exception
    errorFlag = 1;
    errorMsg = exception.message;
    x = NaN(size(lb));
    lambda = struct('reducedCosts', NaN(size(A, 2), 1), 'shadowPrices', NaN(size(A, 1), 1));
    fopt = NaN;
end

extra = struct;

function [x, lambda, fopt, errorFlag, errorMsg, extra] = runclp(f, A, b, lb, ub, sense, constraintTypes, variableTypes, options)

%setup equality, inequality constraints based on constraint types
Aeq = [];
beq = [];
if sum(constraintTypes == 'S') == length(constraintTypes)
    Aeq = A;
    A = [];
    beq = b;
    b = [];
elseif sum(constraintTypes == 'U') == length(constraintTypes)
elseif sum(constraintTypes == 'L') == length(constraintTypes)
    A = -A;
    b = -b;
else
    throw(MException('ComputationUtil:LinearProgramming', 'clp: constraint types invalid'));
end

%throw error if integer or boolean variable types requested, clp
%doesn't have this capability
if sum(variableTypes == 'C') ~= length(variableTypes)
    throw(MException('ComputationUtil:LinearProgramming', 'clp: variable types invalid'));
end

%make structure of clp options
clpoptions = struct;
if isfield(options.solverOptions, 'clp')
    clpoptions = options.solverOptions.clp;
end

try
    [x, dual, status] = clp(zeros(length(f)), sense * f, A, b, Aeq, beq, lb, ub, clpoptions);
    lambda = struct('reducedCosts', NaN(size(A, 2), 1), 'shadowPrices', dual);
    fopt = f' * x;
    errorFlag = status ~= 0;
    switch status
        case 0; errorMsg = 'optimal';
        case 1; errorMsg = 'infeasible';
        case 2; errorMsg = 'unbounded';
    end
catch exception
    errorFlag = 1;
    errorMsg = exception.message;
    x = NaN(size(lb));
    lambda = struct('reducedCosts', NaN(size(A, 2), 1), 'shadowPrices', NaN(size(A, 1), 1));
    fopt = NaN;
end

extra = struct;

function [x, lambda, fopt, errorFlag, errorMsg, extra] = runbpmpd(f, A, b, lb, ub, sense, constraintTypes, variableTypes, ~)

%throw error if integer or boolean variable types requested, bpmpd
%doesn't have this capability
if sum(variableTypes == 'C') ~= length(variableTypes)
    throw(MException('ComputationUtil:LinearProgramming', 'bpmpd: variable types invalid'));
end

%convert constraints types to bpmpd format
if any(constraintTypes == 'F'); throw(MException('ComputationUtil:LinearProgramming', 'bpmpd: free constraints are not allowed')); end;
if any(constraintTypes == 'D'); throw(MException('ComputationUtil:LinearProgramming', 'bpmpd: double-sided constraints are not allowed')); end;
e = NaN(size(constraintTypes));
e(constraintTypes == 'L') = 1;
e(constraintTypes == 'S') = 0;
e(constraintTypes == 'U') = -1;

%throw error if integer or boolean variable types requested, bpmpd
%doesn't have this capability
if sum(variableTypes == 'C') ~= length(variableTypes)
    throw(MException('ComputationUtil:LinearProgramming', 'bpmpd: variable types invalid'));
end

try
    llist = (1:length(lb))';
    ulist = (1:length(ub))';
    [x, dual, ~, w, errorMsg] = bp(zeros(length(f)), A, b, sense * f, e, llist, lb, ulist, ub, bpopt,0);
    lambda = struct('reducedCosts', w, 'shadowPrices', dual);
    fopt = f' * x;
    errorFlag =~ strcmp(errorMsg, 'optimal solution');
catch exception
    errorFlag = 1;
    errorMsg = exception.message;
    x = NaN(size(lb));
    lambda = struct('reducedCosts', NaN(size(A, 2), 1), 'shadowPrices', NaN(size(A, 1), 1));
    fopt = NaN;
end

extra = struct;

function [x, lambda, fopt, errorFlag, errorMsg, extra] = runtomlab( f, A, b, lb, ub, sense, constraintTypes, ~, options)

b_L = -Inf(size(b));
b_U =  Inf(size(b));
b_L(constraintTypes == 'L') = b(constraintTypes == 'L');
b_U(constraintTypes == 'U') = b(constraintTypes == 'U');
b_L(constraintTypes == 'S') = b(constraintTypes == 'S');
b_U(constraintTypes == 'S') = b(constraintTypes == 'S');
b_L(constraintTypes == 'D') = -b(constraintTypes == 'D');
b_U(constraintTypes == 'D') =  b(constraintTypes == 'D');

prob = lpAssign(sense * f, A, b_L, b_U, lb, ub, [], [], [], [], [], [], [], [], []);

solver = 'minos';
prob.optParam = struct();
PriLev = 0;
if isfield(options.solverOptions, 'tomlab')
    if isfield(options.solverOptions.tomlab, 'solver'), solver = options.solverOptions.tomlab.solver; end
    if isfield(options.solverOptions.tomlab, 'optParam'), prob.optParam = options.solverOptions.tomlab.optParam; end
    if isfield(options.solverOptions.tomlab, 'PriLev'), PriLev = options.solverOptions.tomlab.PriLev; end
end

try
    result = tomRun(solver, prob, PriLev);
    errorFlag = result.ExitFlag ~= 0;
    errorMsg = result.ExitText;
    fopt = sense * result.f_k;
    x = result.x_k;
    lambda = struct('reducedCosts', NaN(size(A, 2), 1), 'shadowPrices', NaN(size(A, 1), 1));
catch exception
    errorFlag = 1;
    errorMsg = exception.message;
    x = NaN(size(lb));
    lambda = struct('reducedCosts', NaN(size(A, 2), 1), 'shadowPrices', NaN(size(A, 1), 1));
    fopt = NaN;
end

extra = struct;

function [x, lambda, fopt, errorFlag, errorMsg, extra] = runcplex(f, A, b, lb, ub, sense, constraintTypes, variableTypes, options)
constraintTypes = upper(constraintTypes);
if ~ismembc(constraintTypes, 'LSU')
    throw(MException('ComputationUtil:LinearProgramming', 'linear programming: invalid constraints types'));
end

Aineq = [
    -A(constraintTypes == 'L', :)
    A(constraintTypes == 'U', :)
    ];
bineq = [
    -b(constraintTypes == 'L', :)
    b(constraintTypes == 'U', :)
    ];

Aeq = A(constraintTypes == 'S', :);
beq = b(constraintTypes == 'S', :);

try
    [x, fopt, exitFlag, output] =  cplexmilp(...
        sense * f, ...
        Aineq, bineq, ...
        Aeq, beq, ...
        [], [], [], lb, ub, variableTypes');
    errorFlag = exitFlag ~= 1;
    errorMsg = output.message;
catch exception
    errorFlag = 1;
    errorMsg = exception.message;
    x = NaN(size(lb));
    fopt = NaN;
end
lambda = struct('reducedCosts', NaN(size(A, 2), 1), 'shadowPrices', NaN(size(A, 1), 1));

extra = struct('status', output.cplexstatus);

function [x, lambda, fopt, errorFlag, errorMsg, extra] = rungurobi( f, A, b, lb, ub, sense, constraintTypes, variableTypes, options)
b_L = -Inf(size(b));
b_U =  Inf(size(b));
b_L(constraintTypes == 'L') = b(constraintTypes == 'L');
b_U(constraintTypes == 'U') = b(constraintTypes == 'U');
b_L(constraintTypes == 'S') = b(constraintTypes == 'S');
b_U(constraintTypes == 'S') = b(constraintTypes == 'S');
b_L(constraintTypes == 'D') = -b(constraintTypes == 'D');
b_U(constraintTypes == 'D') =  b(constraintTypes == 'D');

xint = NaN(size(variableTypes));
xint(variableTypes == 'I') = 1;
xint(variableTypes == 'C') = 0;

PriLev = 0;
grbControl = struct();
if isfield(options.solverOptions, 'gurobi')
    if isfield(options.solverOptions.gurobi, 'PriLev'), PriLev = options.solverOptions.gurobi.PriLev; end
    if isfield(options.solverOptions.gurobi, 'grbControl'), grbControl = options.solverOptions.gurobi.grbControl; end
end

try
    [x, ~, v, rc, fopt, ~, ~, status] = ...
        gurobi(sense * f, A, lb, ub, b_L, b_U, [], ...
        grbControl, PriLev, xint);
    fopt = fopt * sense;
    [errorMsg, exitFlag] = grbStatus(status);
    errorFlag = exitFlag ~= 0;
    lambda = struct('reducedCosts', rc, 'shadowPrices', v);
catch exception
    errorFlag = 1;
    errorMsg = exception.message;
    x = NaN(size(lb));
    lambda = struct('reducedCosts', NaN(size(A, 2), 1), 'shadowPrices', NaN(size(A, 1), 1));
    fopt = NaN;
end

extra = struct;

function [x, lambda, fopt, errorFlag, errorMsg, extra] = runmosek( f, A, b, lb, ub, sense, constraintTypes, ~, options)
if sense == 1
    cmd = 'minimize';
else
    cmd = 'maximize';
end

b_L = -Inf(size(b));
b_U =  Inf(size(b));
b_L(constraintTypes == 'L') = b(constraintTypes == 'L');
b_U(constraintTypes == 'U') = b(constraintTypes == 'U');
b_L(constraintTypes == 'S') = b(constraintTypes == 'S');
b_U(constraintTypes == 'S') = b(constraintTypes == 'S');
b_L(constraintTypes == 'D') = -b(constraintTypes == 'D');
b_U(constraintTypes == 'D') =  b(constraintTypes == 'D');

lb = max(lb, -1e7);
ub = min(ub,  1e7);

opts = struct(...
    'MSK_IPAR_LOG', 0, 'MSK_IPAR_MAX_NUM_WARNINGS', 1e5, ...
    'MSK_IPAR_INTPNT_SCALING', 0, 'MSK_IPAR_SIM_SCALING', 0, 'MSK_IPAR_SIM_SCALING_METHOD', 1);
if isfield(options.solverOptions, 'mosek')
    opts = options.solverOptions.mosek;
end

try
    result = msklpopt(f, A, b_L, b_U, lb, ub, opts, cmd);
    fopt = result.sol.itr.pobjval;
    x = result.sol.itr.xx;
    errorFlag = result.rcode ~= 0;
    errorMsg = result.rmsg;
    lambda = struct(...
        'reducedCosts', result.sol.itr.slx + result.sol.itr.sux, ...
        'shadowPrices', result.sol.itr.slc + result.sol.itr.suc);
catch exception
    errorFlag = 1;
    errorMsg = exception.message;
    x = NaN(size(lb));
    lambda = struct('reducedCosts', NaN(size(A, 2), 1), 'shadowPrices', NaN(size(A, 1), 1));
    fopt = NaN;
end

extra = struct;


classdef ICMA < ALGORITHM
% <multi> <real/integer> <constrained>
% Indicator-based constrained multi-objective algorithm

%------------------------------- Reference --------------------------------
% J. Yuan, H. Liu, Y. Ong, and Z. He, Indicator-based evolutionary
% algorithm for solving constrained multi-objective optimization problems,
% IEEE Transactions on Evolutionary Computation, 2022, 26(2): 379-391.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Jiawei Yuan

    methods
        function main(Algorithm,Problem)
            %% Generate the random population
            Population = Problem.Initialization();
            Zmin       = min(Population.objs,[],1);
            Fmin       = min(Population(all(Population.cons<=0,2)).objs,[],1);
            Archive    = Population;
            W          = UniformPoint(Problem.N,Problem.M);
            Ra         = 1;

            %% Optimization
            while Algorithm.NotTerminated(Archive)
                Nt = floor(Ra*Problem.N);
                MatingPool = [Population(randsample(Problem.N,Nt)),Archive(randsample(Problem.N,Problem.N-Nt))];

                [Mate1,Mate2,Mate3] = Neighbor_Pairing_Strategy(MatingPool,Zmin);
                if rand > 0.5
                    Offspring = OperatorDE(Problem,Mate1,Mate2,Mate3);
                else
                    Offspring = OperatorDE(Problem,Mate1,Mate2,Mate3,{0.5,0.5,0.5,0.75});
                end

                Fmin = min([Fmin;Offspring(all(Offspring.cons<=0,2)).objs],[],1);
                Zmin = min([Zmin;Offspring.objs],[],1);
                [Population,Archive] = ICMA_Update([Population,Offspring,Archive],Problem.N,W,Zmin,Fmin);

                Ra = 1 - Problem.FE/Problem.maxFE;
            end
        end
    end
end


% Author: Ricardo Baptista and Matthias Poloczek
% Date:   June 2018
%
% See LICENSE.md for copyright information
%

function particle_system = move_particles(particle_system)
% MOVE_PARTICLES: Function moves the current particles according to an MCMC
% kernel with an adaptive logistic regression proposal.

% Extract function inputs
n_vars     = particle_system.n_vars;
model_orig = particle_system.models;
model_val  = particle_system.model_val;
rho        = particle_system.rho;
logistic_A = particle_system.logistic_A;

% Determine the number of current models, and dimensions
[n_models, ~] = size(model_orig);

% Compute the old particle diversity ratio
diversity_old = unique_particles(model_orig);

% Declare vector for accept_ratios
mh_accept_vect = [];

total_fn_count = particle_system.total_fn_count(end);

iter_temp = 0;
while(1)

    % Generate matrix to store new samples and data
    new_model = zeros(n_models, n_vars);
    new_mval  = zeros(n_models, 1);

    % Declare vector to store acceptance ratio
    accept_ratio = 0;

    for i=1:n_models

        % Propose new model
        [prop_model, ~] = logistic_sample(n_vars, logistic_A);

        % Evaluate proposal density under both models
        q_new = evaluate_logit(particle_system, prop_model);
        q_old = evaluate_logit(particle_system, model_orig(i,:));

        % Evaluate probability function for both models
        total_fn_count = total_fn_count + 1;
        model_obj_new = particle_system.objective(prop_model);
        log_post_new = rho*model_obj_new;
        log_post_old = rho*model_val(i);

        % Compute acceptance probability
        accept_prob = exp(log_post_new - log_post_old)*(q_old/q_new);

        % Accept proposed sample/data or assign old values
        if rand < min(1,accept_prob)
            new_model(i,:) = prop_model;
            new_mval(i)    = model_obj_new;
            accept_ratio   = accept_ratio + 1;
        else
            new_model(i,:) = model_orig(i,:);
            new_mval(i)    = model_val(i);
        end

    end

    %% Update Sample Information

    % Update variables
    model_orig     = new_model;
    model_val      = new_mval;
    mh_accept_vect = [mh_accept_vect, accept_ratio/n_models];

    % Compute the new particle diversity
    diversity_new = unique_particles(new_model);

    % Compare the diversity
    if (abs(diversity_new - diversity_old) < 0.02 || diversity_new > 0.95)
        break
    end

    % Update diversity_old
    diversity_old = diversity_new;

    iter_temp = iter_temp + 1;
    if iter_temp > 100
        break
    end

end

% Update particle_system
particle_system.models     = model_orig;
particle_system.model_val  = model_val;
particle_system.mh_accept  = min(mh_accept_vect);
particle_system.total_fn_count = [particle_system.total_fn_count, total_fn_count];

end


function M = slpweval(X1, X2, f, varargin)
%SLPWEVAL Perform pairwise computation
%
% $ Syntax $
%   - M = slpweval(X1, X2, f)
%   - M = slpweval(X1, X2, f, ...)
%
% $ Arguments $
%   - X1:       the matrix of vectors serving as first argument of f
%   - X2:       the matrix of vectors serving as second argument of f
%   - f:        the function maps two vectors to a single scalar value
%   - M:        the matrix of pairwise evaluaton results
%
% $ Description $
%   - M = slpweval(X1, X2, f) takes vector arguments from X1 and X2, and
%     computes the pairwise evaluation result with f. The vectors in X1
%     and X2 are stored in a column-wise manner. Suppose X1 and X2 have
%     m and n columns respectively, then the resultant matrix M would be
%     of size m x n, with the M(i, j) = f(X1(:,i), X2(:,j)).
%
%   - M = slpweval(X1, X2, f, ...) conducts the computation with extra
%     parameters to f, i.e. M(i, j) = f(X1(:,i), X2(:,j), ...).
%
% $ Remarks $
%   - The vector length of the vectors in X1 and X2 are not necessarily
%     equal. The requirment on their dimensions depends on the callback
%     function f.
%   - For efficiency, the function would invoke f to evaluate in batch.
%     Thus f should support batch-evaluation. When the input arguments
%     to f have n columns, f should return an 1 x n row vector.
%
% $ History $
%   - Created by Dahua Lin on Apr 21, 2006
%   - Modified by Dahua Lin on Sep 10, 2006
%       - make some minor changes to suppress warnings
%

%% parse and verify input arguments

if nargin < 3
    raise_lackinput('slpweval', 3);
end
[d1, n1] = size(X1);
[d2, n2] = size(X2);
slignorevars(d1, d2);

%% compute

% prepare output matrix
M = zeros(n1, n2);

if n1 > n2      % expand each column in X2 to n1 copies

    inds_e = ones(1, n1);
    for i = 1 : n2
        x2 = X2(:, i);
        X2e = x2(:, inds_e);
        M(:, i) = feval(f, X1, X2e, varargin{:})';
    end

else            % expand each column in X1 to n2 copies

    inds_e = ones(1, n2);
    for i = 1 : n1
        x1 = X1(:, i);
        X1e = x1(:, inds_e);
        M(i, :) = feval(f, X1e, X2, varargin{:});
    end

end


function blas1_d_test08 ( )

%*****************************************************************************80
%
%% TEST08 tests DROTG.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    John Burkardt
%
  test_num = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST08\n' );
  fprintf ( 1, '  DROTG generates a real Givens rotation\n' );
  fprintf ( 1, '    (  C  S ) * ( A ) = ( R )\n' );
  fprintf ( 1, '    ( -S  C )   ( B )   ( 0 )\n' );
  fprintf ( 1, '\n' );

  seed = 123456789;

  for test = 1 : test_num

    [ a, seed ] = r8_uniform_01 ( seed );
    [ b, seed ] = r8_uniform_01 ( seed );

    [ c, s, r, z ] = drotg ( a, b );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  A =  %f  B =  %f\n', a, b );
    fprintf ( 1, '  C =  %f  S =  %f\n', c, s );
    fprintf ( 1, '  R =  %f  Z =  %f\n', r, z );
    fprintf ( 1, '   C*A+S*B = %f\n',  c * a + s * b );
    fprintf ( 1, '  -S*A+C*B = %f\n', -s * a + c * b );

  end

  return
end


function inside = p15_inside ( m, n, point )

%*****************************************************************************80
%
%% P15_INSIDE reports if a point is inside the region in problem 15.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, real POINT(M,N), the coordinates of the points.
%
%    Output, logical INSIDE(N), is TRUE if the point is in the region.
%
  inside(1:n) =                                          ...
       ( -8.0 <= point(1,1:n) & point(1,1:n) <= 2.0 &    ...
         -1.0 <= point(2,1:n) & point(2,1:n) <= 0.0    ) ...
    |                                                    ...
       ( -2.0 <= point(1,1:n) & point(1,1:n) <= 8.0 &    ...
          0.0 <= point(2,1:n) & point(2,1:n) <= 1.0 );

  return
end


function out = regress(dat, varargin)
% Multiple regression with an fmri_data object (dat), predicting brain data with a design matrix stored in dat.X (or vice versa)
%
% Regress dat.X on dat.dat at each voxel, and return voxel-wise statistic
% images. Each column of dat.X is a predictor in a multiple regression,
% and the intercept is the last column. Intercept will automatically be
% added if not detected unless 'nointercept' is specified.
%
% Key pointers:
% - Output structure (regression_results_ols) contains beta and t images in statistic_image objects
%
% - If contrasts are entered, output structure contains contrast images and contrast t images in statistic_image objects
%
% - T images are thresholded, but beta/contrast images are not. These can be re-thresholded with the threshold( ) method
%
% - statistic_image objects can be visuallized with any CANlab display methods, e.g., surface( ), montage( ), and orthviews( )
%
% - Use the get_wh_image( ) method to select specific beta, t, contrast, contrast t images from statistic_image objects
%
% - Enter condition names and contrast names as cell vectors of strings Labels are saved in the .image_labels property in output objects
%
% - Some diagnostics, including VIFs and leverages, are automatically checked and returned
%
% - There is an option for robust regression, specified using the 'robust' flag
%
% - For first-level models (only), the grandmeanscale option is recommended to increase homogeneity in scale across participants
%
% - The regress( ) method does not use covariates field of fmri_data(). You must include covariates
%    manually in dat.X.
%
% - The regress( ) method can also create a map of brain regions that predict the dat.Y vector using the 'brainony' option.
%   This is essentially a univariate version of the 'predict' command.  Warning: this is very slow as it loops
%   through all voxels.
%
% Creates thresholded plot by default
%
% :Usage:
% ::
%
%    out = regress(dat, varargin)
%
% :Inputs:
%  **dat:**
%        should be an fmri_data object with X field defined.
%        dat.X can be a design_matrix() object.
%
% :Optional Inputs:
%  **[threshold, 'unc']:**
%        p-value threshold string indicating threshold type
%        (see help statistic_image.threshold for options)
%
%  **robust:**
%        Run a robust regression (default is OLS).  Robust is considerably
%        slower than OLS
%
%  **grandmeanscale:**
%        Scale overall grand mean to a value of 100. Intended to reduce inter-subject
%        variability in 1st-level analysis (single-subject) when doing
%        multi-subject group analyses on resulting contrasts/beta images.
%        Assumes mask and overall brain size are consistent across replicates (e.g., participants, in a first-level analysis)
%        Do not use for 2nd-level or standard single-level analyses.
%
%  **C**
%       Followed by a contrast matrix, each column is a contrast across conditions/events
%       [k x c] matrix, where k = size(X, 2) and c is number of contrasts
%
%       Note: k must be the number including the intercept. If your input
%       model X does not include an intercept, contrast matrix must have
%       one more element than input X has rows, to account for the added
%       intercept.
%
%  **nointercept:**
%        Do not add intercept to model
%
%  **display, display_results:**
%        Show thresholded results usin orthviews (if < 10 regressors)
%
%  **nodisplay:**
%        Do not plot thresholded results using orthviews
%
%  **brainony:**
%        univariate approach to predict obj.Y from brain data
%
%  **residual:**
%        Output residual as fmri_data() object
%
%  **noverbose:**
%        Suppress verbose outputs
%
%  **variable_names:** (or 'names')
%       Followed by a cell array of variable/regressor names, for
%       non-intercept regressors
%
%  **contrast_names:**
%       Followed by a cell array of contrast names
%
%  **analysis_name:**
%       Followed by a string with a name/description for this analysis.
%
%
% :Outputs:
%
%  **out:**
%        A structure containing stats_img and fmri_data objects.
%        In addition to the main outputs below, the out structure also has
%        fields for input_parameters, the design matrix (X), variable
%        names, and warnings.
%
%  **out.b:**
%        stats_img object of beta values estimated from regression
%
%  **out.t:**
%        stats_img object of t-values with input threshold
%
%  **out.df:**
%        fmri_data object of degrees of freedom
%
%  **out.sigma:**
%        fmri_data object of variance of residual
%
%  **out.residual:**
%        fmri_data object of residual data after model has been regressed out (optional).
%
%  **out.diagnostics:***
%        A structure containing VIFs and leverage values for the design matrix
%        out.diagnostics.Variance_inflation_factors = VIFs
%        out.diagnostics.Leverages = leverage values
%
% :Examples:
% ::
%
%    % Run regression with liberal threshold
%    out = regress(dat, .05, 'unc');
%
%    % Run regression with conservative threshold and save residual
%    out = regress(dat, .001, 'unc', 'residual);
%
%    % Run robust regression with fdr threshold
%    out = regress(dat, .05, 'fdr','robust');
%
%    % Run a regression predicting behavior from brain at liberal threshold
%    out  = regress(data_comb, .05, 'unc', 'brainony')
%
%    % Re-threshold at different values
%    out.t = threshold(out.t, .05, 'fdr');
%    out.t = threshold(out.t, .001, 'unc');
%
%    % Re-display results of thresholding
%    orthviews(out.t);
%
%    % Write out beta image to current directory
%    out.b.fullpath = fullfile(pwd,'beta.nii');
%    write(out)
%
%    % Plot diagnostics
%    figure; subplot(1,2,1); title('VIFs')
%    plot(regression_results.diagnostics.Variance_inflation_factors);
%    subplot(1,2,2); title('Leverage of each observation')
%    plot(regression_results.diagnostics.Leverages);
%
%   % Run with options:
%   regression_results = regress(dat, 'variable_names', names, 'analysis_name', 'Pinel localizer 1st-level GLM', 'noverbose');
%
%   regression_results_ols = regress(dat, 'variable_names', names, ...
%     'analysis_name', 'Pinel localizer 1st-level GLM', ...
%     'C', C.weights, 'contrast_names', C.names);
%
% Show some results for selected individual conditions
% % Thresholded t map for the first condition
% t = get_wh_image(regression_results_ols.t, 1);
%
% create_figure('montage'); axis off
% display_obj = montage(t);
%
% figure; surface(t);
%
% % Montages for thresholded t images, condition [2 4 6 8]
%
% t = get_wh_image(regression_results_ols.t, [2 4 6 8]);
%
% create_figure('montage2'); axis off
% display_obj = montage(t);
% Show  results for contrasts
% t = get_wh_image(regression_results_ols.con_t, 1:3);
%
% create_figure('montage3'); axis off
% display_obj = montage(t);
%
% t = get_wh_image(regression_results_ols.con_t, 4:6);
%
% create_figure('montage4'); axis off
% display_obj = montage(t);

% ..
%    Copyright (c) 2015 Tor Wager & Luke Chang
%
%    Permission is hereby granted, free of charge, to any person obtaining a
%    copy of this software and associated documentation files (the "Software"),
%    to deal in the Software without restriction, including without limitation
%    the rights to use, copy, modify, merge, publish, distribute, sublicense,
%    and/or sell copies of the Software, and to permit persons to whom the
%    Software is furnished to do so, subject to the following conditions:
%
%    The above copyright notice and this permission notice shall be included
%    in all copies or substantial portions of the Software.
%
%    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
%    OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
%    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
%    THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
%    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
%    FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
%    DEALINGS IN THE SOFTWARE.
% ..

% ..
%    Programmers' Notes:
%    c Tor Wager, Dec 2010
%    Edited by Luke Chang, 9/27/2012 to add optional input to reverse X & Y (i.e., create a map of voxels that predict the behavioral variable)
%    Edited by Luke Chang, 9/28/2012 to add optional input to run robust regression for brainony
%    Edited by Luke Chang, 10/24/2012 to save residuals (i.e., out.r), which is helpful for denoising an image
%    Edited by Luke Chang, 3/26/2013 to add optional input to not add an intercept - allows for more flexible modeling options
%    Code completely refactored by Luke Chang 2/24/25
%    Verbose option updated by Tor, 7/2015
%    Help, some outputs, robust % update: added by Tor, 5/2021
% ..

% ..
%    ---------------------------------------------------------------------
%    Defaults
%    ---------------------------------------------------------------------
% ..
inputargs = {.001, 'uncorrected'}; % default options for thresholding
do_display = false;
brain_is_outcome = 1; %else do brain on Y
do_robust = 0;
do_intercept = 1;
do_resid = 0;
doverbose = true;
variable_names = {};
contrast_names = {};
analysis_name = '';
C = [];
grandmeanscale = false;

% ---------------------------------------------------------------------
% Parse Inputs
% ---------------------------------------------------------------------
for varg = 1:length(varargin)

    if ischar(varargin{varg})
        % reserved keywords
        if strcmpi('unc',varargin{varg})
            inputargs = {varargin{varg-1}, 'uncorrected'};
            varargin{varg} = {}; varargin{varg - 1} = {};
        end
        if strcmpi('fdr',varargin{varg})
            inputargs = {varargin{varg-1}, 'fdr'};
            varargin{varg} = {}; varargin{varg - 1} = {};
        end

        if strcmpi('nodisplay',varargin{varg})
            do_display = 0;
            varargin{varg} = {};
        end

        if strcmpi('display',varargin{varg}) | strcmpi('display_results',varargin{varg})
            do_display = 0;
            varargin{varg} = {};
        end

        if strcmpi('robust',varargin{varg})
            do_robust = 1;
            varargin{varg} = {};
        end

        if strcmpi('brainony',varargin{varg}) | strcmpi('brain_is_predictor',varargin{varg}) %#ok<*OR2>
            brain_is_outcome = 0;
            varargin{varg} = {};
        end
        if strcmpi('nointercept',varargin{varg})
            do_intercept = 0;
            varargin{varg} = {};
        end
        if strcmpi('residual',varargin{varg})
            do_resid = 1;
            varargin{varg} = {};
        end
        if strcmpi('noverbose',varargin{varg})
            doverbose = false;
            varargin{varg} = {};
        end

        if strcmpi('names',varargin{varg}) | strcmpi('variable_names',varargin{varg})
            variable_names = varargin{varg + 1};
            varargin{varg} = {}; varargin{varg + 1} = {};
        end

        if strcmpi('C',varargin{varg}) | strcmpi('contrasts',varargin{varg})
            C = varargin{varg + 1};
            varargin{varg} = {}; varargin{varg + 1} = {};
        end

        if strcmpi('contrast_names',varargin{varg})
            contrast_names = varargin{varg + 1};
            varargin{varg} = {}; varargin{varg + 1} = {};
        end

        if strcmpi('analysis_name',varargin{varg})
            analysis_name = varargin{varg + 1};
            varargin{varg} = {}; varargin{varg + 1} = {};
        end

        if strcmpi('grandmeanscale',varargin{varg})
            grandmeanscale = true;
            varargin{varg} = {};
        end

    end % if ischar
end

if ~doverbose % add to pass into threshold( )
    inputargs{end+1} = 'noverbose';
end

% ---------------------------------------------------------------------
% Check Data and Diagnostics
% ---------------------------------------------------------------------

mywarnings = {};

% Check if fmri_data or image_vector
% ---------------------------------------------------------------------

if ~isa(dat,'fmri_data')
    error('dat input must be fmri_data object')
end

% Check data bit rate
% ---------------------------------------------------------------------

nuniquevals = length(unique(dat.dat(:)));

if nuniquevals < 2^10
    mywarnings{end+1} = sprintf('Number of unique values in dataset is low (%d, %3.2f bits), indicating possible restriction of bit rate. For comparison, Int16 has 65,536 unique values', nuniquevals, log2(nuniquevals));
end

% Check of dat.X exists and is correct format
% ---------------------------------------------------------------------

if brain_is_outcome
    if isempty(dat.X)
        error('Make sure you include a design matrix in dat.X')
    end
    if size(dat.dat, 2) ~= size(dat.X, 1)
        error('dat.dat must have same number of columns as dat.X has rows.')
    end
    if isa(dat.X,'design_matrix')
        dat.X = dat.X.dat;
    end
else % Check if dat.Y exists and is in correct format if running brainony
    if isempty(dat.Y)
        error('Make sure you include a vector in dat.Y.')
    end
    if size(dat.dat, 2) ~= size(dat.Y, 1)
        error('dat.dat must have same number of columns as dat.Y has rows.')
    end
end

% Check if Rank Deficient
% ---------------------------------------------------------------------

if rank(dat.X) < size(dat.X,2)
    mywarnings{end+1} = 'Warning:  dat.X is rank deficient.';
end

intercept_string = 'intercept is last';

% Check if Intercept is in model or specified for x_on_brain default
% ---------------------------------------------------------------------

if do_intercept && brain_is_outcome
    wh_int = intercept(dat.X, 'which');

    if isempty(wh_int)
        % add intercept and update wh_int (used later)
        if doverbose, mywarnings{end+1} = 'No intercept detected, adding intercept to last column of design matrix'; end
        X = intercept(dat.X, 'add');
        variable_names{end + 1} = 'Intercept';
%         wh_int = intercept(X, 'which');

    else
        intercept_string = sprintf('Intercept detected in column %1.0f of dat.X', wh_int);

        if doverbose, mywarnings{end+1} = intercept_string; end
        X = dat.X;

    end

else
    % No intercept or exogenous variables are outcome

    X = dat.X;
end

% Predictor centering
% ---------------------------------------------------------------------

m = mean(X);
wh_int = intercept(X, 'which');
m(wh_int) = [];
non_centered = abs(m) > 100 * eps;

% For effects-coded values [-1 1], ok, we want the intercept to reflect stats at average of 2 groups
iseffectcode = all(abs(X) == 1); % for each column
iseffectcode(wh_int) = [];

if any(non_centered & iseffectcode)

    mywarnings{end+1} = 'Warning:  Group sizes are unequal for effects-coded [1 -1] variable.';

end

if any(non_centered & ~iseffectcode)
    mywarnings{end+1} = 'Warning:  Predictors are not centered -- intercept is not interpretable as stats for average subject';
end

% Variance inflation
% ---------------------------------------------------------------------

vifs = getvif(X);

if any(vifs > 4)

    mywarnings{end+1} = 'Warning!!!  Design multicolinearity. Some regressors have variance inflation factors > 4. Check out.diagnostics';

end

% Leverages
% ---------------------------------------------------------------------

H = X*pinv(X);
%H = X*inv(X'*X)*X'  will be identical if not rank deficient
leverages = diag(H);

if any(abs(zscore(leverages)) >= 3)
     mywarnings{end+1} = 'Warning!!!  Some observations have high leverage values relative to others, regression may be unstable. abs(z(leverage)) > 3';
end

% Names
% ---------------------------------------------------------------------

k = size(X, 2);
if length(variable_names) < k
   if ~isempty(variable_names), mywarnings{end+1} = 'Warning!!!  Too few variable names entered, less than size(X, 2). Names may be inaccurate.'; end % suppress warning if NO names entered

   for i = length(variable_names)+1:k %#ok<*FXUP>
       variable_names{i} = sprintf('R%d', i); %#ok<*AGROW>
   end
end

if length(variable_names) > k
   mywarnings{end+1} = 'Warning!!!  Too many variable names entered, more than size(X, 2). Names may be inaccurate.';

   variable_names = variable_names(1:k);
end

% Check contrasts
% ---------------------------------------------------------------------

if ~isempty(C) && ~(size(C, 1) == size(X, 2))
    % Do this *after* adding intercept to X if needed

    disp('Contrasts entered, but size(C, 1) does not equal size(X, 2).');
    disp('Must have a contrast entry for each column of X (including the intercept)');
    error('Quitting.')

end

% Contrast names
% ---------------------------------------------------------------------

if ~isempty(C)

    kc = size(C, 2);

    if length(contrast_names) < kc
        if ~isempty(contrast_names), mywarnings{end+1} = 'Warning!!!  Too few contrast names entered, less than size(C, 2). Names may be inaccurate.'; end % suppress warning if NO names entered

        for i = length(contrast_names)+1:kc
            contrast_names{i} = sprintf('Con%d', i); %#ok<*AGROW>
        end
    end

    if length(contrast_names) > kc
        mywarnings{end+1} = 'Warning!!!  Too many contrast names entered, more than size(C, 2). Names may be inaccurate.';

        contrast_names = contrast_names(1:kc);
    end

end

% Enforce proper shape
if ~iscolumn(variable_names), variable_names = variable_names'; end
if ~isrow(contrast_names), contrast_names = contrast_names'; end

% Enforce valid names: Eliminate special characters and leading numbers
[variable_names, namewarnings] = format_text_letters_only(variable_names, 'numbers', 'cleanup', 'squeeze', 'underscore_ok');
mywarnings = [mywarnings namewarnings];
if ~isempty(namewarnings), mywarnings{end+1} = 'Enter valid variable_names'; end

if ~isempty(C)

    [contrast_names, namewarnings] = format_text_letters_only(contrast_names, 'numbers', 'cleanup', 'squeeze', 'underscore_ok');
    mywarnings = [mywarnings namewarnings];
    if ~isempty(namewarnings), mywarnings{end+1} = 'Enter valid contrast_names'; end

end

% Data scaling and format
% ---------------------------------------------------------------------

% Enforce double-format (just in case)
dat.dat = double(dat.dat);

if grandmeanscale

    % Scale grand mean to 100; approximates what SPM and other packages do. Assumes mask and overall brain size are consistent across replicates (e.g., participants, in a first-level analysis
    dat.dat = dat.dat .* 100 / nanmean(dat.dat(:));

end

if doverbose

    fprintf('Analysis: %s\n', analysis_name);
    disp('----------------------------------');

    nowarnings = all(cellfun(@isempty, mywarnings));

    disp('Design matrix warnings:');
    disp('----------------------------------');
    if nowarnings
        disp('None')

    else
        disp(char(mywarnings{:}))

    end

    disp(' ');
end

% ---------------------------------------------------------------------
% Run Regression
% ---------------------------------------------------------------------

tic
warning off

if brain_is_outcome
    % default is to regress dat.X on dat.dat (x on brain)
    % ---------------------------------------------------------------------

    % display info about regression
    if doverbose
        linestr = '______________________________________________________';
        disp(linestr);

        fprintf('Running regression: %3.0f voxels. Design: %3.0f obs, %3.0f regressors, %s\n', size(dat.dat, 1), size(X, 1), size(X, 2), intercept_string);
        if brain_is_outcome
            fprintf('\nPredicting exogenous variable(s) in dat.X using brain data as predictors, mass univariate');

        else % default
            fprintf('\nPredicting Brain Activity from dat.X, mass univariate');
        end
    end

    if do_robust
        % Robust - Regress stim/behavior(X) on brain (Y)
        %need to loop through voxels - Slow!

        if doverbose
            fprintf('\nRunning in Robust Mode ___%%');
        end

        v = size(dat.dat, 1);
        [n, k] = size(X);

        % Initialize outputs
        [b, t, stderr] = deal(zeros(k, v));
        p = ones(k, v);
        [dfe, sigma] = deal(zeros(1, v));

        for i = 1:v
            % For each voxel

            [bb,stats] = robustfit(X, dat.dat(i,:)', 'bisquare', [], 'off');

            b(:,i)=bb; %Betas
            t(:,i)=stats.t; %t-values
            p(:,i)=stats.p; %p-values
            dfe(:,i)=stats.dfe; %degrees of freedom
            stderr(:,i)=stats.se; %Standard Error
            sigma(:,i)=stats.robust_s; %robust estimate of sigma. LC not sure this is the best choice can switch to 'OLS_s','MAD_s', or 's'

            if doverbose && mod(i, 100) == 0
                fprintf('\b\b\b\b%03d%%', 100 * round(i/v))
            end
        end
        r = dat.dat' - X*b; %residual

    else
        % OLS - X predicting brain
        % - vectorized - Fast!

        if doverbose, fprintf('\nRunning in OLS Mode'); end

        % Estimate Betas in vector

        b = pinv(X) * dat.dat';

        % Error
        r = dat.dat' - X*b;

        % Residual variance
        [stderr, sigma] = get_std_errors(r, X);

        % Inference
        [t,dfe,p,sigma] = param_t_test(X,b,stderr,sigma);

    end

else
    % Regress brain (X) on stim/behavior (Y) - loops through voxels, slow!
    % ---------------------------------------------------------------------

    if doverbose
        % display info about regression
        fprintf('regression > X: %3.0f voxels. Y: %3.0f obs, %3.0f regressors, %s\n', size(dat.dat, 1), size(dat.Y, 2), intercept_string);
        fprintf('\nPredicting dat.Y from Brain Activity');
    end

    if do_robust %need to loop through voxels - Slow!
        if doverbose
            fprintf('\nRunning in Robust Mode ___%%');
        end

        v = size(dat.dat, 1);
        n = size(dat.dat, 2);
        k = 2; % one predictor (brain) and an intercept

        % Initialize outputs
        [b, t, stderr] = deal(zeros(k, v));
        p = ones(k, v);
        [dfe, sigma] = deal(zeros(1, v));

        for i = 1:v
            % Create X from brain Data
            if do_intercept
                X = intercept(dat.dat(i,:)','add');
            else
                X = dat.dat(i,:)';
            end

            [bb,stats] = robustfit(X, dat.Y, 'bisquare', [], 'off');

            b(:,i)=bb; %Betas
            t(:,i)=stats.t; %t-values
            p(:,i)=stats.p; %p-values
            dfe(:,i)=stats.dfe; %degrees of freedom
            stderr(:,i)=stats.se; %Standard Error
            sigma(:,i)=stats.robust_s; %robust estimate of sigma. LC not sure this is the best choice can switch to 'OLS_s','MAD_s', or 's'
            r(:,i) = dat.Y - X * b(:,i); %residual

            if doverbose && mod(i, 100) == 0
                fprintf('\b\b\b\b%03d%%', 100 * round(i/v))
            end

        end % voxel

    else
        % ---------------------------------------------------------------------
        %OLS -- Regress brain on Y

        if doverbose, fprintf('\nRunning in OLS Mode'); end

        for i = 1:size(dat.dat,1)

            % Create X from brain Data
            if do_intercept
                X = intercept(dat.dat(i,:)','add');
            else
                X = dat.dat(i,:)';
            end

            % Estimate Betas in vector
            b(:,i) = pinv(X) * dat.Y;

            % Error
            r(:,i) = dat.Y - X * b(:,i);

            %             sigma(i) = std(r(:,i)); % wrong
            %             stderr(:,i) = ( diag(inv(X' * X)) .^ .5 ) * sigma(i);  % params x voxels matrix of std. errors
        end

        % Residual variance - can do this at end because X is always the same size
        [stderr, sigma] = get_std_errors(r, X);

        % Inference
        [t,dfe,p,sigma] = param_t_test(X,b,stderr,sigma);

    end % dorobust

end % brain_is_outcome or not

stop = toc;
if doverbose, fprintf('\nModel run in %d minutes and %.2f seconds\n',floor(stop/60),rem(stop,60)); end

% ---------------------------------------------------------------------
% Contrasts
% ---------------------------------------------------------------------

if ~isempty(C)

    con_vals = C' * b;

    % Contrast STE
    con_ste = diag(C' * inv(X' * X) * C) .^ .5 * sigma;

    [con_t, ~, con_p] = param_t_test(X, con_vals, con_ste, sigma);

end

% ---------------------------------------------------------------------
% Create Output
% ---------------------------------------------------------------------

out = struct;

out.analysis_name = analysis_name;

out.input_parameters = struct( ...
    'brain_is_predictor', brain_is_outcome, 'do_robust', do_robust, 'grandmeanscale', grandmeanscale, ...
    'do_intercept', do_intercept, ...
    'do_resid', do_resid, 'doverbose', doverbose, 'do_display', do_display);

out.input_parameters.initial_statistical_threshold = inputargs;

out.input_image_metadata.source_notes = dat.source_notes;
out.input_image_metadata.history = dat.history;
out.input_image_metadata.image_names = dat.image_names;
out.input_image_metadata.fullpath = dat.fullpath;

% design, contrasts, and diagnostics

out.X = X;
out.variable_names = variable_names;
out.C = C;
out.contrast_names = contrast_names;

out.contrast_summary_table = table();

if ~isempty(C)
    % Contrast summary table

    for i = 1:size(C, 2)
        out.contrast_summary_table(:, i) = table(C(:, i));
    end

    out.contrast_summary_table.Properties.RowNames = variable_names;
    out.contrast_summary_table.Properties.VariableNames = contrast_names;

    if doverbose
        fprintf('\nSummary of conditions and contrasts\n%s\n', linestr);
        disp(out.contrast_summary_table);
    end

end

out.diagnostics = struct('Variance_inflation_factors', vifs, 'Leverages', leverages);
out.warnings = mywarnings;

% Create objects
if doverbose
    fprintf('\nCreating beta and t images, thresholding t images\n%s\n', linestr);
end

% Betas
out.b = statistic_image;
out.b.type = 'Beta';
out.b.p = p';
out.b.ste = stderr';
out.b.N = n;
out.b.dat = b';
out.b.dat_descrip = sprintf('Beta Values from regression, intercept is column %d', wh_int);
out.b.volInfo = dat.volInfo;
out.b.removed_voxels = dat.removed_voxels;
out.b.removed_images = false;  % this image does not have the same dims as the original dataset
out.b.image_labels = variable_names;

out.b = enforce_variable_types(out.b);

% - beta and contrast images are unthresholded, t images from both are thresholded
% if doverbose, fprintf('Thresholding b images at %3.6f %s\n', inputargs{1}, inputargs{2}); end
% out.b = threshold(out.b, inputargs{:}, 'noverbose'); % Threshold image

% T stats
out.t = statistic_image;
out.t.type = 'T';
out.t.p = p';
out.t.ste = stderr';
out.t.N = n;
out.t.dat = t';
out.t.dat_descrip = sprintf('t-values from regression, intercept is column %d', wh_int);
out.t.volInfo = dat.volInfo;
out.t.removed_voxels = dat.removed_voxels;
out.t.removed_images = false;  % this image does not have the same dims as the original dataset
out.t.image_labels = variable_names;

out.t = enforce_variable_types(out.t);

if doverbose
    fprintf('Thresholding t images at %3.6f %s\n', inputargs{1}, inputargs{2});
end
out.t = threshold(out.t, inputargs{:}); %Threshold image

% DF as fmri_data
out.df = dat;
out.df.dat = dfe';
out.df.dat_descrip = sprintf('Degrees of Freedom');

% Sigma as fmri_data
out.sigma = dat;
out.sigma.dat = sigma';
out.sigma.dat_descrip = sprintf('Sigma from Regression');

% Residual as fmri_data
if do_resid
    out.resid = dat;
    out.resid.dat = r';
    out.resid.dat_descrip = sprintf('Residual from Regression');
end

if ~isempty(C)

    if doverbose, fprintf('\nCreating contrast and t images and thresholding t images\n%s\n', linestr); end

    % Contrast values
    out.contrast_images = statistic_image;
    out.contrast_images.type = 'Contrast';
    out.contrast_images.p = con_p';
    out.contrast_images.ste = stderr';
    out.contrast_images.N = n;
    out.contrast_images.dat = con_vals';
    out.contrast_images.dat_descrip = 'Contrast Values from regression';
    out.contrast_images.volInfo = dat.volInfo;
    out.contrast_images.removed_voxels = dat.removed_voxels;
    out.contrast_images.removed_images = false;  % this image does not have the same dims as the original dataset
    out.contrast_images.image_labels = contrast_names;

    out.contrast_images = enforce_variable_types(out.contrast_images);

%     if doverbose, fprintf('Thresholding contrast images at %3.6f %s\n', inputargs{1}, inputargs{2}); end
%     out.contrast_images = threshold(out.contrast_images, inputargs{:}, 'noverbose'); % Threshold image

    % T stats
    out.con_t = statistic_image;
    out.con_t.type = 'T';
    out.con_t.p = con_p';
    out.con_t.ste = con_ste';
    out.con_t.N = n;
    out.con_t.dat = con_t';
    out.con_t.dat_descrip = sprintf('t-values from regression, intercept is column %d', wh_int);
    out.con_t.volInfo = dat.volInfo;
    out.con_t.removed_voxels = dat.removed_voxels;
    out.con_t.removed_images = false;  % this image does not have the same dims as the original dataset
    out.con_t.image_labels = contrast_names;

    out.con_t = enforce_variable_types(out.con_t);

    if doverbose
        fprintf('Thresholding t images at %3.6f %s\n', inputargs{1}, inputargs{2});

    end
    out.con_t = threshold(out.con_t, inputargs{:}); %Threshold image

end

% ---------------------------------------------------------------------
% Plot Results
% ---------------------------------------------------------------------
if k < 10 && do_display

    orthviews(out.t);

elseif do_display && doverbose
    disp('Warning: No display because >= 10 images.');

end

warning on

% ---------------------------------------------------------------------
% Subfunctions
% ---------------------------------------------------------------------

    function [stderr, sigma] = get_std_errors(r, X)

        [n, k] = size(X);
        v = size(dat.dat, 1);

        % We want diag(r' * r), but matrix size can be large
        % std(r) does not account for k parameters used, so is incorrect
        % residual std. not sqrt(var(resid)) -- we must account for k params used
        for i = 1:v
            sigma(1, i) = (r(:, i)' * r(:, i) ./ (n - k)) .^ .5;
        end

        stderr = ( diag(inv(X' * X)) .^ .5 ) * sigma;  % params x voxels matrix of std. errors

    end

    function [t,dfe,p,sigma] = param_t_test(X,b,stderr,sigma)
        % test whether parameter is significantly different from zero
        %
        % Inputs:
        % X:        design matrix
        % b:        beta values
        % stderr:   standard error of beta estimate
        % sigma:    standard deviation of residual
        %
        % Returns:
        % t:        t-value
        % dfe:      degrees of freedom
        % p:        p-value
        % sigma:    standard deviation of residual

        [n, k] = size(X);
        t = b ./ stderr;
        dfe = n - k;
        p = 2 * (1 - tcdf(abs(t), dfe));

        sigma(sigma == 0) = Inf;
        t(isnan(t)) = 0;
        p(isnan(p)) = 0;
        dfe = repmat(dfe,1,size(t,2));
    end

end % Main Function


function I = getPcontrolCurrentPack(t,t0,tf,x,param,extra)
%	getPcontrolCurrentPack returns the value of the input current as a function of
%	the time, states and parameters
%
%       I = getPcontrolCurrentPack(t,t0,tf,y,param,extra)
%
%       Inputs:
%               - t         : value of the current time step
%               - t0        : initial integration time
%               - tf        : final integration time
%               - x         : contains the array of all the states
%                             (differential and algebraic) at time t
%               - param     : contains the parameters structure
%               - extra     : extra parameters
%       Outputs:
%               - I     : Applied current desnity [A/m^2]

%   This file is part of the LIONSIMBA Toolbox
%
%	Official web-site: 	http://sisdin.unipv.it/labsisdin/lionsimba.php
% 	Official GitHUB: 	https://github.com/lionsimbatoolbox/LIONSIMBA
%
%   LIONSIMBA: A Matlab framework based on a finite volume model suitable for Li-ion battery design, simulation, and control
%   Copyright (C) 2016-2018 :Marcello Torchio, Lalo Magni, Davide Raimondo,
%                            University of Pavia, 27100, Pavia, Italy
%                            Bhushan Gopaluni, Univ. of British Columbia,
%                            Vancouver, BC V6T 1Z3, Canada
%                            Richard D. Braatz,
%                            Massachusetts Institute of Technology,
%                            Cambridge, Massachusetts 02142, USA
%
%   Main code contributors to LIONSIMBA 2.0:
%                           Ian Campbell, Krishnakumar Gopalakrishnan,
%                           Imperial college London, London, UK
%
%   LIONSIMBA is a free Matlab-based software distributed with an MIT
%   license.

% This script provides the value of the applied current density as a
% function of the voltage across the battery pack.

% Firstly we extract the variables related to the first and second cell. To
% this aim the field x_indices of the param structure contains the values
% of the absolute positionong of the variables in the overall x array.
% Indeed, when running simulations with multiple cells, the states array
% (x) will contain as many rows - for a given time instant t - as the sum
% of the states of all the cells involved in the simulation. x_indices
% stores, for each cell, what are the indices of the overall x in which the
% variables of a given cell are stored.

% Extract the first cell variables
cell1_variables = x(param{1}.x_index);
% Extract the second cell variables
cell2_variables = x(param{2}.x_index);

% At this stage, since we have put the variables of the 2 cells into
% cell1_variables and cell2_variables, use their relative indices to
% extract the exact values.

% Cell 1 voltage
V_1 = cell1_variables(param{1}.Phis_indices(1))-cell1_variables(param{1}.Phis_indices(end));

% Cell 2 voltage
V_2 = cell2_variables(param{2}.Phis_indices(1))-cell2_variables(param{2}.Phis_indices(end));

% Define the proportional action. Do not put extreme values, the simulator
% could crash
Kp = 100;

% Define the Voltage Setpoint
V_ref = 8.2;
% Define your linear or nonlinear function of t for evaluate the value
% of I.
I = Kp*(V_ref-(V_1+V_2));

end


function [ o, x, w ] = en_r2_09_1 ( n, option )

%*****************************************************************************80
%
%% EN_R2_09_1 implements the Stroud rule 9.1 for region EN_R2.
%
%  Discussion:
%
%    The rule has order O = ( 2 * N^4 - 4 * N^3 + 22 * N^2 - 8 * N + 3 ) / 3.
%
%    The rule has precision P = 9.
%
%    EN_R2 is the entire N-dimensional space with weight function
%
%      w(x) = exp ( - x1^2 - x2^2 ... - xn^2 )
%
%    There are two versions of each rule, chosen by setting the
%    OPTION variable to 1 or 2.
%
%    The rule as tabulated by Stenger is available for N = 2 through 20.
%    This function accepts N = 3 through 6.
%
%     N    O
%    __  ___
%     3   77
%     4  193
%     5  421
%     6  825
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 January 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971,
%    ISBN: 0130438936,
%    LC: QA311.S85.
%
%  Parameters:
%
%    Input, integer N, the spatial dimension.
%    3 <= N <= 6.
%
%    Input, integer OPTION, chooses rule option 1 or 2.
%
%    Output, integer O, the order.
%
%    Output, real X(N,O), the abscissas.
%
%    Output, real W(O), the weights.
%
  if ( n < 3 | 6 < n )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'EN_R2_09_1 - Fatal error!\n' );
    fprintf ( 1, '  3 <= N <= 6 required.\n' );
    error ( 'EN_R2_09_1 - Fatal error!' )
  end

  if ( nargin < 2 )
    option = 1;
  end

  if ( option < 1 | 2 < option )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'EN_R2_09_1 - Fatal error!\n' );
    fprintf ( 1, '  1 <= OPTION <= 2 required.\n' );
    error ( 'EN_R2_09_1 - Fatal error!' )
  end

  o = ( 2 * n^4 - 4 * n^3 + 22 * n^2 - 8 * n + 3 ) / 3;
  volume = sqrt ( pi^n );

  if ( n == 3 )
    u =    0.202018287045609E+01;
    v =    0.958572464613819E+00;
    b0 =   0.676448734429924E+00;
    b1 =   0.511989106291551E-02;
    b2 =   0.448595723493744E+00;
    b3 =   0.235223454595606E-03;
    b4 =   0.915390713080005E-01;
    b5 =   0.139208199920793E-01;
    b6 =   0.235223454595606E-03;
    b7 =   0.915390713080008E-01;
    b8 =   0.000000000000000E+00;
  elseif ( n == 4 & option == 1 )
    u =    0.202018287045609E+01;
    v =    0.958572464613819E+00;
    b0 = - 0.860452945007048E+00;
    b1 = - 0.405511998533795E-01;
    b2 =   0.107026475449715E+01;
    b3 =   0.138974239307092E-03;
    b4 = - 0.162248779448181E+00;
    b5 =   0.246740110027234E-01;
    b6 =   0.138974239307094E-03;
    b7 =   0.162248779448181E+00;
    b8 =   0.138974239307094E-03;
  elseif ( n == 4 & option == 2 )
    u =    0.958572464613819E+00;
    v =    0.202018287045609E+01;
    b0 =   0.265029088766810E-02;
    b1 =   0.637601342635332E+00;
    b2 = - 0.394394059389228E-01;
    b3 =   0.540829264827264E-01;
    b4 = - 0.416922717921281E-03;
    b5 =   0.246740110027234E-01;
    b6 =   0.540829264827270E-01;
    b7 =   0.416922717921281E-03;
    b8 =   0.540829264827269E-01;
  elseif ( n == 5 & option == 1 )
    u =    0.202018287045609E+01;
    v =    0.958572464613819E+00;
    b0 = - 0.827347006200826E+01;
    b1 = - 0.160820174530905E+00;
    b2 =   0.353499863758467E+01;
    b3 =   0.738976276909564E-03;
    b4 = - 0.862735421812943E+00;
    b5 =   0.437335458190621E-01;
    b6 = - 0.246325425636523E-03;
    b7 =   0.287578473937648E+00;
    b8 =   0.246325425636523E-03;
  elseif ( n == 5 & option == 2 )
    u =    0.958572464613819E+00;
    v =    0.202018287045609E+01;
    b0 = - 0.624416791055272E+00;
    b1 =   0.467494915583104E+00;
    b2 = - 0.152937760910536E+00;
    b3 =   0.287578473937646E+00;
    b4 = - 0.221692883072871E-02;
    b5 =   0.437335458190621E-01;
    b6 = - 0.958594913125490E-01;
    b7 =   0.738976276909568E-03;
    b8 =   0.958594913125492E-01;
  elseif ( n == 6 & option == 1 )
    u =    0.202018287045609E+01;
    v =    0.958572464613819E+00;
    b0 = - 0.361840434143098E+02;
    b1 = - 0.447936529138517E+00;
    b2 =   0.112077863004144E+02;
    b3 =   0.392940404320855E-02;
    b4 = - 0.254859786784158E+01;
    b5 =   0.775156917007496E-01;
    b6 = - 0.130980134773619E-02;
    b7 =   0.509719573568315E+00;
    b8 =   0.436600449245395E-03;
  elseif ( n == 6 & option == 2 )
    u =    0.958572464613819E+00;
    v =    0.202018287045609E+01;
    b0 =   0.448873836333650E+01;
    b1 = - 0.238473566140736E+01;
    b2 = - 0.413008493198885E+00;
    b3 =   0.152915872070494E+01;
    b4 = - 0.654900673868093E-02;
    b5 =   0.775156917007496E-01;
    b6 = - 0.509719573568314E+00;
    b7 =   0.130980134773618E-02;
    b8 =   0.169906524522772E+00;
  end

  x = zeros(n,o);
  w = zeros(o,1);

  k = 0;
%
%  1 point.
%
  k = k + 1;
% x(1:n,k) = 0.0;
  w(k) = b0;
%
%  2 * N points.
%
  for i = 1 : n
    k = k + 1;
    x(i,k) = - u;
    w(k) = b1;
    k = k + 1;
    x(i,k) = + u;
    w(k) = b1;
  end
%
%  2 * N points.
%
  for i = 1 : n
    k = k + 1;
    x(i,k) = - v;
    w(k) = b2;
    k = k + 1;
    x(i,k) = + v;
    w(k) = b2;
  end
%
%  4 * ( N * ( N - 1 ) / 2 ) points.
%
  for i = 1 : n - 1
    for j = i + 1 : n
      k = k + 1;
      x(i,k) = - u;
      x(j,k) = - u;
      w(k) = b3;
      k = k + 1;
      x(i,k) = - u;
      x(j,k) = + u;
      w(k) = b3;
      k = k + 1;
      x(i,k) = + u;
      x(j,k) = - u;
      w(k) = b3;
      k = k + 1;
      x(i,k) = + u;
      x(j,k) = + u;
      w(k) = b3;
    end
  end
%
%  4 * ( N * ( N - 1 ) / 2 ) points.
%
  for i = 1 : n - 1
    for j = i + 1 : n
      k = k + 1;
      x(i,k) = - v;
      x(j,k) = - v;
      w(k) = b4;
      k = k + 1;
      x(i,k) = - v;
      x(j,k) = + v;
      w(k) = b4;
      k = k + 1;
      x(i,k) = + v;
      x(j,k) = - v;
      w(k) = b4;
      k = k + 1;
      x(i,k) = + v;
      x(j,k) = + v;
      w(k) = b4;
    end
  end
%
%  4 * ( N * ( N - 1 ) ) points.
%
  for i = 1 : n - 1
    for j = i + 1 : n
      k = k + 1;
      x(i,k) = - u;
      x(j,k) = - v;
      w(k) = b5;
      k = k + 1;
      x(i,k) = - u;
      x(j,k) = + v;
      w(k) = b5;
      k = k + 1;
      x(i,k) = + u;
      x(j,k) = - v;
      w(k) = b5;
      k = k + 1;
      x(i,k) = + u;
      x(j,k) = + v;
      w(k) = b5;
      k = k + 1;
      x(i,k) = - v;
      x(j,k) = - u;
      w(k) = b5;
      k = k + 1;
      x(i,k) = - v;
      x(j,k) = + u;
      w(k) = b5;
      k = k + 1;
      x(i,k) = + v;
      x(j,k) = - u;
      w(k) = b5;
      k = k + 1;
      x(i,k) = + v;
      x(j,k) = + u;
      w(k) = b5;
    end
  end
%
%  8 * ( N * ( N - 1 ) * ( N - 2 ) / 6 ) points.
%
  for i = 1 : n - 2
    for j = i + 1 : n - 1
      for l = j + 1 : n
        k = k + 1;
        x(i,k) = - u;
        x(j,k) = - u;
        x(l,k) = - u;
        w(k) = b6;
        k = k + 1;
        x(i,k) = - u;
        x(j,k) = - u;
        x(l,k) = + u;
        w(k) = b6;
        k = k + 1;
        x(i,k) = - u;
        x(j,k) = + u;
        x(l,k) = - u;
        w(k) = b6;
        k = k + 1;
        x(i,k) = - u;
        x(j,k) = + u;
        x(l,k) = + u;
        w(k) = b6;
        k = k + 1;
        x(i,k) = + u;
        x(j,k) = - u;
        x(l,k) = - u;
        w(k) = b6;
        k = k + 1;
        x(i,k) = + u;
        x(j,k) = - u;
        x(l,k) = + u;
        w(k) = b6;
        k = k + 1;
        x(i,k) = + u;
        x(j,k) = + u;
        x(l,k) = - u;
        w(k) = b6;
        k = k + 1;
        x(i,k) = + u;
        x(j,k) = + u;
        x(l,k) = + u;
        w(k) = b6;
      end
    end
  end
%
%  8 * ( N * ( N - 1 ) * ( N - 2 ) / 6 ) points.
%
  for i = 1 : n - 2
    for j = i + 1 : n - 1
      for l = j + 1 : n
        k = k + 1;
        x(i,k) = - v;
        x(j,k) = - v;
        x(l,k) = - v;
        w(k) = b7;
        k = k + 1;
        x(i,k) = - v;
        x(j,k) = - v;
        x(l,k) = + v;
        w(k) = b7;
        k = k + 1;
        x(i,k) = - v;
        x(j,k) = + v;
        x(l,k) = - v;
        w(k) = b7;
        k = k + 1;
        x(i,k) = - v;
        x(j,k) = + v;
        x(l,k) = + v;
        w(k) = b7;
        k = k + 1;
        x(i,k) = + v;
        x(j,k) = - v;
        x(l,k) = - v;
        w(k) = b7;
        k = k + 1;
        x(i,k) = + v;
        x(j,k) = - v;
        x(l,k) = + v;
        w(k) = b7;
        k = k + 1;
        x(i,k) = + v;
        x(j,k) = + v;
        x(l,k) = - v;
        w(k) = b7;
        k = k + 1;
        x(i,k) = + v;
        x(j,k) = + v;
        x(l,k) = + v;
        w(k) = b7;
      end
    end
  end
%
%  16 * ( N * ( N - 1 ) * ( N - 2 ) * ( N - 3 ) / 24 ) points.
%
  for i = 1 : n - 3
    for j = i + 1 : n - 2
      for l = j + 1 : n - 1
        for m = l + 1 : n
          k = k + 1;
          x(i,k) = - u;
          x(j,k) = - u;
          x(l,k) = - u;
          x(m,k) = - u;
          w(k) = b8;
          k = k + 1;
          x(i,k) = - u;
          x(j,k) = - u;
          x(l,k) = - u;
          x(m,k) = + u;
          w(k) = b8;
          k = k + 1;
          x(i,k) = - u;
          x(j,k) = - u;
          x(l,k) = + u;
          x(m,k) = - u;
          w(k) = b8;
          k = k + 1;
          x(i,k) = - u;
          x(j,k) = - u;
          x(l,k) = + u;
          x(m,k) = + u;
          w(k) = b8;
          k = k + 1;
          x(i,k) = - u;
          x(j,k) = + u;
          x(l,k) = - u;
          x(m,k) = - u;
          w(k) = b8;
          k = k + 1;
          x(i,k) = - u;
          x(j,k) = + u;
          x(l,k) = - u;
          x(m,k) = + u;
          w(k) = b8;
          k = k + 1;
          x(i,k) = - u;
          x(j,k) = + u;
          x(l,k) = + u;
          x(m,k) = - u;
          w(k) = b8;
          k = k + 1;
          x(i,k) = - u;
          x(j,k) = + u;
          x(l,k) = + u;
          x(m,k) = + u;
          w(k) = b8;
          k = k + 1;
          x(i,k) = + u;
          x(j,k) = - u;
          x(l,k) = - u;
          x(m,k) = - u;
          w(k) = b8;
          k = k + 1;
          x(i,k) = + u;
          x(j,k) = - u;
          x(l,k) = - u;
          x(m,k) = + u;
          w(k) = b8;
          k = k + 1;
          x(i,k) = + u;
          x(j,k) = - u;
          x(l,k) = + u;
          x(m,k) = - u;
          w(k) = b8;
          k = k + 1;
          x(i,k) = + u;
          x(j,k) = - u;
          x(l,k) = + u;
          x(m,k) = + u;
          w(k) = b8;
          k = k + 1;
          x(i,k) = + u;
          x(j,k) = + u;
          x(l,k) = - u;
          x(m,k) = - u;
          w(k) = b8;
          k = k + 1;
          x(i,k) = + u;
          x(j,k) = + u;
          x(l,k) = - u;
          x(m,k) = + u;
          w(k) = b8;
          k = k + 1;
          x(i,k) = + u;
          x(j,k) = + u;
          x(l,k) = + u;
          x(m,k) = - u;
          w(k) = b8;
          k = k + 1;
          x(i,k) = + u;
          x(j,k) = + u;
          x(l,k) = + u;
          x(m,k) = + u;
          w(k) = b8;
        end
      end
    end
  end

  return
end


function varargout = diff(varargin)
%DIFF   Derivative of a CHEBFUN2 object.
%   DIFF(F) is the derivative of F along the y direction.
%
%   DIFF(F, N) is the Nth derivative of F in the y direction.
%
%   DIFF(F, N, DIM) is the Nth derivative of F along the dimension DIM.
%     DIM = 1 (default) is the derivative in the y-direction.
%     DIM = 2 is the derivative in the x-direction.
%
%   DIFF(F, [NX NY]) is the partial derivative of NX of F in the first variable,
%   and NY of F in the second derivative. For example, DIFF(F,[1 2]) is
%   d^3F/dxd^2y.
%
% See also GRADIENT, SUM, PROD.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

[varargout{1:nargout}] = diff@separableApprox(varargin{:});

end


function [z_irm,z_ibm,t] = applyIdealMasks(xt,xi,nfft,hop,fs)
%APPLYIDEALMASKS Calculate and apply ideal masks via STFT
%
%   Z_IRM = IOSR.BSS.APPLYIDEALMASKS(XT,XI) calculates the ideal ratio mask
%   (IRM) and applies it to the mixture XT+XI, where XT is the target
%   signal and XI is the interference signal. The IRM is calculated via the
%   STFT using 1024-point windows with 512-point overlap. Z_IRM, XT, and XI
%   are vectors. If XT and XI are of different lengths then the shorter
%   signal is zero-padded in order to make them the same length; Z_IRM is
%   the same length as XT and XI.
%
%   Z_IRM = IOSR.BSS.APPLYIDEALMASKS(XT,XI,NFFT) uses NFFT-length segments
%   in the STFT.
%
%   Z_IRM = IOSR.BSS.APPLYIDEALMASKS(XT,XI,WINDOW) uses
%   LENGTH(WINDOW)-length segments in the STFT and applies WINDOW to each
%   segment.
%
%   Z_IRM = IOSR.BSS.APPLYIDEALMASKS(XT,XI,WINDOW,HOP) uses hop size HOP
%   for the STFT.
%
%   [Z_IRM,Z_IBM] = IOSR.BSS.APPLYIDEALMASKS(...) calculates the ideal
%   binary mask (IBM) and applies it to the mixture, returning the result
%   to Z_IBM.
%
%   [Z_IRM,Z_IBM,T] = IOSR.BSS.APPLYIDEALMASKS(XT,XI,WINDOW,HOP,FS) uses
%   sampling frequency FS to return the corresponding time T of each
%   element in Z_IRM and Z_IBM.
%
%   See also IOSR.DSP.STFT, IOSR.DSP.ISTFT, IOSR.BSS.IDEALMASKS,
%            IOSR.BSS.APPLYMASKS.

%   Copyright 2016 University of Surrey.

    %% check input

    % check signals
    assert(isvector(xt) && numel(xt)>1, 'iosr:applyIdealMasks:invalidXt', 'XT must be a vector')
    assert(isvector(xi) && numel(xi)>1, 'iosr:applyIdealMasks:invalidXi', 'XI must be a vector')

    % make equal length
    maxlength = max([length(xi) length(xt)]);
    xt = pad(xt,maxlength);
    xi = pad(xi,maxlength);

    % check nfft
    if nargin<3
        nfft = 1024;
    end

    % determine window
    if numel(nfft)>1
        win = nfft;
        assert(isvector(win), 'iosr:applyIdealMasks:invalidWin', 'WINDOW must be a vector')
        nfft = length(win);
    else
        assert(round(nfft)==nfft && nfft>0, 'iosr:applyIdealMasks:invalidNfft', 'NFFT must be a positive integer')
        win = hamming(nfft);
    end

    % check x length
    assert(length(xt)>=nfft, 'iosr:applyIdealMasks:invalidXt', 'XT must have at least NFFT samples')
    assert(length(xi)>=nfft, 'iosr:applyIdealMasks:invalidXi', 'XI must have at least NFFT samples')

    % determine hop
    if nargin<4
        hop = fix(nfft/2);
    else
        assert(isscalar(hop) & round(hop)==hop, 'iosr:applyIdealMasks:invalidHop', 'HOP must be an integer')
        assert(hop<=nfft && hop>0, 'iosr:applyIdealMasks:invalidHop', 'HOP must be less than or equal to NFFT, and greater than 0')
    end

    % determine fs
    if nargin<5
        fs = 1;
    else
        assert(isscalar(fs), 'iosr:applyIdealMasks:invalidFs', 'FS must be an scalar')
    end

    %% calculate outputs

    % STFTs of signals and mixture
    st = iosr.dsp.stft(xt,win,hop);
    si = iosr.dsp.stft(xi,win,hop);
    mix = iosr.dsp.stft(xt+xi,win,hop);

    % return ideal masks
    [irm,ibm] = iosr.bss.idealMasks(st,si);

    % apply IRM
    z_irm = iosr.bss.applyMask(mix,irm,nfft,hop,fs);
    z_irm = pad(z_irm,maxlength);

    % apply IBM
    if nargout>1
        z_ibm = iosr.bss.applyMask(mix,ibm,nfft,hop,fs);
        z_ibm = pad(z_ibm,maxlength);
    end

    % calculate t
    if nargout>2
        t = (0:length(z_irm)-1)./fs;
    end

end

function y = pad(x,dur)
%PAD Zero-pad a vector

    if length(x)<dur
        y = [x(:); zeros(dur-length(x),1)];
    else
        y = x(:);
    end

end


% ------------------------------------------------------------------------
%  Copyright (C)
%  Universitat Politecnica de Catalunya BarcelonaTech (UPC) - Spain
%  University of California Berkeley (UCB) - USA
%
%  Jordi Pont-Tuset <jordi.pont@upc.edu>
%  Pablo Arbelaez <arbelaez@berkeley.edu>
%  June 2014
% ------------------------------------------------------------------------
% This file is part of the MCG package presented in:
%    Arbelaez P, Pont-Tuset J, Barron J, Marques F, Malik J,
%    "Multiscale Combinatorial Grouping,"
%    Computer Vision and Pattern Recognition (CVPR) 2014.
% Please consider citing the paper if you use this code.
% ------------------------------------------------------------------------
function [int_area, int_bbox] = boxes_intersection( bbox1, bbox2 )
    up1    = bbox1(1);
    left1  = bbox1(2);
    down1  = bbox1(3);
    right1 = bbox1(4);

    up2    = bbox2(1);
    left2  = bbox2(2);
    down2  = bbox2(3);
    right2 = bbox2(4);

    int_left  = max(left1,left2);
    int_right = min(right1,right2);
    int_up    = max(up1,up2);
    int_down  = min(down1,down2);

    if (int_left<=int_right) && (int_up<=int_down)
        int_bbox = [int_up, int_left, int_down, int_right];
        int_area = box_area(int_bbox);
    else
        int_bbox = [];
        int_area = 0;
    end
end


%SerialLink.IKUNC Inverse manipulator by optimization without joint limits
%
% Q = R.ikunc(T, OPTIONS) are the joint coordinates (1xN) corresponding to
% the robot end-effector pose T which is an SE3 object or homogenenous
% transform matrix (4x4), and N is the number of robot joints. OPTIONS is
% an optional list of name/value pairs than can be passed to fminunc.
%
% Q = robot.ikunc(T, Q0, OPTIONS) as above but specify the
% initial joint coordinates Q0 used for the minimisation.
%
% [Q,ERR] = robot.ikunc(T,...) as above but also returns ERR which is the
% scalar final value of the objective function.
%
% [Q,ERR,EXITFLAG] = robot.ikunc(T,...) as above but also returns the
% status EXITFLAG from fminunc.
%
% [Q,ERR,EXITFLAG,OUTPUT] = robot.ikunc(T,...) as above but also returns the
% structure OUTPUT from fminunc which contains details about the optimization.
%
% Trajectory operation::
%
% In all cases if T is a vector of SE3 objects (1xM) or a homogeneous transform
% sequence (4x4xM) then returns the joint coordinates corresponding to
% each of the transforms in the sequence.  Q is MxN where N is the number
% of robot joints. The initial estimate of Q for each time step is taken as
% the solution from the previous time step.
%
% ERR and EXITFLAG are also Mx1 and indicate the results of optimisation
% for the corresponding trajectory step.
%
% Notes::
% - Requires fminunc from the MATLAB Optimization Toolbox.
% - Joint limits are not considered in this solution.
% - Can be used for robots with arbitrary degrees of freedom.
% - In the case of multiple feasible solutions, the solution returned
%   depends on the initial choice of Q0
% - Works by minimizing the error between the forward kinematics of the
%   joint angle solution and the end-effector frame as an optimisation.
%   The objective function (error) is described as:
%           sumsqr( (inv(T)*robot.fkine(q) - eye(4)) * omega )
%   Where omega is some gain matrix, currently not modifiable.
%
% Author::
% Bryan Moutrie
%
% See also SerialLink.ikcon, fmincon, SerialLink.ikine, SerialLink.fkine.

% Copyright (C) Bryan Moutrie, 2013-2015
% Licensed under the GNU Lesser General Public License
% see full file for full statement
%
% LICENSE STATEMENT:
%
% This file is part of pHRIWARE.
%
% pHRIWARE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as
% published by the Free Software Foundation, either version 3 of
% the License, or (at your option) any later version.
%
% pHRIWARE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU Lesser General Public
% License along with pHRIWARE.  If not, see <http://www.gnu.org/licenses/>.

function [qstar, error, exitflag, output] = ikunc(robot, T, varargin)

    % check if Optimization Toolbox exists, we need it
    assert( exist('fminunc', 'file')>0, 'rtb:ikunc:nosupport', 'Optimization Toolbox required');

    if isa(T, 'SE3')
        T = T.T;
    end

    % create output variables
    T_sz = size(T,3);
    qstar = zeros(T_sz,robot.n);
    error = zeros(T_sz,1);
    exitflag = zeros(T_sz,1);

    problem.solver = 'fminunc';
    problem.x0 = zeros(1, robot.n);
    problem.options = optimoptions('fminunc', ...
        'Algorithm', 'quasi-newton', ...
        'Display', 'off'); % default options for ikunc

    if nargin > 2
        % check if there is a q0 passed
        if isnumeric(varargin{1}) && length(varargin{1}) == robot.n
            problem.x0 = varargin{1};
            varargin = varargin(2:end);
        end
    end
    if ~isempty(varargin)
        % if given, add optional argument to the list of optimiser options
        problem.options = optimoptions(problem.options, varargin{:});
    end

    reach = sum(abs([robot.a, robot.d]));
    omega = diag([1 1 1 3/reach]);

    for t = 1:T_sz
        problem.objective = ...
            @(x) sumsqr(((T(:,:,t) \ robot.fkine(x).T) - eye(4)) * omega);

        [q_t, err_t, ef_t, out_t] = fminunc(problem);

        if ef_t ~= 1
            if T_sz > 1
                warning('step %d: errflag = %d, err = %f\n', t, ef_t, err_t);
            else
                warning('errflag = %d, err = %f\n', ef_t, err_t);
                out_t
            end
        end
        qstar(t,:) = q_t;
        error(t) = err_t;
        exitflag(t) = ef_t;
        output(t) = out_t;

        problem.x0 = q_t;
    end

end

function s = sumsqr(A)
    s = sum(A(:).^2);
end