function [Contain]=BuildMPS(A, b, Aeq, beq, cost, L, U, PbName, varargin)
%
% function Contain=BuildMPS(A, b, Aeq, beq, cost, L, U, PbName); OR
%          Contain=BuildMPS(..., Param1, Value1, ...);
%
% Build ascii fixed-width MPS matrix string that contains linear
% programming (LP) problem:
%
% Minimizing (for x in R^n): f(x) = cost'*x, subject to
%       A*x <= b        (LE)
%       Aeq*x = beq     (EQ)
%       L <= x <= U     (BD).
%
% Also supported is integer/mixte programming problem similar to the above,
% where a subset of components of x is restricted to be integer (N set) or
% binary set {0,1}.
%
% INPUTS:
%   A: (m x n) matrix
%   b: (m x 1) matrix
%   Aeq: (k x n) matrix
%   beq: (k x 1) matrix
%   cost: (n x 1) matrix
%   L: (1 x n), (n x 1) or (1 x 1)
%   U: (1 x n), (n x 1) or (1 x 1)
%
% Remark: To disable constraint(s) (LE, EQ, BD), please use empty []
%         for corresponding input matrix/rhs parameters.
%
% Optional:
%   - PbName is a string of problem name, default value is 'GENERIC'.
% Other Params:
%    'EleNames', 'EqtNames', or 'VarNames'
%     Cells contain string of respectively
%        (LE) equations, (EQ) equations, or variable names
%   - 'EleNameFun', 'EqtNameFun', or 'VarNameFun'
%     Corresponding Value are function handles that return
%     Equation/Variable name from equation/Variable number
%       Example: > VarNameFun=@(m) char('x'+(m-1));
%     These functions will NOT be used if names of equations/variables
%     are defined.
%   - Param is 'MPSfilename': output MPS file to be saved
%     No saving if MPSfilename is undefined.
%   - 'I', 'Int', 'Integer', 'Integers'
%       Array that stores the indexes that defines the set of integer
%       variables (>=0). The indexes must belong to [1,..., n] and
%       correspond to the column of A, Aeq.
%   - 'B', 'Bin', 'Binary', 'Binaries'
%       Array that stores the index that defines the set of binary
%       variables {0,1}. Indexes follow the same convention as with integer
%       case.
%   - 'QUAD', 'Q': a structure with following fields
%         'Q': (n x n) matrix
%            The lower triangle of Q is assumed to be the transpose of the
%            upper triangle (in other word, Q must be symmetric and we use
%            only the upper part).
%         'g': (n x 1) vector
%         'bquad': scalar
%         'name' (optional): string, name of the constraint.
%                if qs.name is 'COST' then the quadratic term in the
%                cost function to be minimized (see below)
%         'type' (optional): must contains the string 'QLE'
%                (This field is reserved for future for extension of BuildMPS)
%
%      QUAD parameters is used to enforce an additional quadratic
%      constraint on the unknown x of the type:
%           0.5*x'*Q*x + g'*x <= bquad        (QLE)
%
%      Provide as many QUAD parameters as the number of constraints to be
%      meet.
%      Spatial case: if name is 'COST' then it corresponds to a quadratic
%      term of the cost function
%           f(x) = cost'*x + 0.5*x'*Q*x
%      For this case, the 'g' and 'bquad' fields will be ignored.
%
% OUTPUT:
%   Contain: char matrix of the MPS format description of LP/IP problem.
%
% RESTRICTION:
%   Only single column rhs (b and beq) is supported.
%
% The MPS (Mathematical Programming System) file format was introduced by
% IBM in 1970s, but has also been accepted by most subsequent linear
% programming codes. To learn about MPS format, please see:
%   http://lpsolve.sourceforge.net/5.5/mps-format.htm
%
% See also: SaveMPS
%
% Usage example:
%
%     A = [1 1 0; -1 0 -1];
%     b = [5; -10];
%     L = [0; -1; 0];
%     U = [4; +1; +inf];
%     Aeq = [0 -1 1];
%     beq = 7;
%     cost = [1 4 9];
%     VarNameFun = @(m) (char('x'+(m-1))); % returning varname 'x', 'y' 'z'
%
%     Qle = [2 1 0;
%          1 2 0;
%          0 0 1];
%     g = [0; 0; -3];
%     bquad = 100;
%     quad_le = struct('Q', Qle, ...
%                      'g', g, ...
%                      'bquad', bquad);
%
%     Qcost = speye(3);
%     quad_cost = struct('Q', Qcost, ...
%                        'name', 'cost'),
%     Contain = BuildMPS(A, b, Aeq, beq, cost, L, U, 'Pbtest', ...
%                        'VarNameFun', VarNameFun, ...
%                        'EqtNames', {'Equality'}, ...
%                        'Q', quad_le, 'Q', quad_cost, ...
%                        'Integer', [1], ... % first variable 'x' integer
%                        'MPSfilename', 'Pbtest.mps');
%
% Author: Bruno Luong
% update: 15-Jul-2008: sligly improved number formatting
%         25-Aug-2009: Improvement in handling sparse matrix
%         03-Sep-2009: integer/binary variables
%         02-May-2010: quadratic term

if nargin<8 || isempty(PbName)
    PbName='GENERIC';
end

%
% Columns indices of MPS fields
%
idx1=02:03;
idx2=05:12;
idx3=15:22;
idx4=25:36;
idx5=40:47;
idx6=50:61;
idxlist={idx1 idx2 idx3 idx4 idx5 idx6};

%
% Default returned value if error occurs
%
Contain=[]; %#ok
OK = 0;  %#ok

%
% Get the size of the input matrices
%
[neq nvar]=size(Aeq);
[nle sizeA2]=size(A);

if neq==0 % Aeq is empty, i.e., no equality constraint
    nvar=sizeA2;
    Aeq=zeros(0,nvar);
elseif nle==0 % A is empty, i.e., no LE constraint
    sizeA2=nvar;
    A=zeros(0,nvar);
end

%
% Default values for naming functions (nested functions)
%
elenamefun = @elename;
eqtnamefun = @eqtname;
varnamefun = @varname;
MPSfilename = ''; % MPSfilename

% default empty integer and binary sets
iset = [];
bset = [];

%
% Parse options (varargin)
%
parseoptions(varargin{:});

% Number of quadratic constraints
nquadle = length(quadle);

if ~exist('elenames','var')
    elenames=arrayfun(elenamefun, (1:nle), 'UniformOutput', false);
end
if ~exist('eqtnames','var')
    eqtnames=arrayfun(eqtnamefun, (1:neq), 'UniformOutput', false);
end
if ~exist('varnames','var')
    varnames=arrayfun(varnamefun, (1:nvar), 'UniformOutput', false);
end

if nargin<6 || isempty(L)
    L=-inf(1,nvar);
elseif isscalar(L) % extend L if it's a scalar input
    Lval=L;
    L=zeros(1,nvar);
    L(:)=Lval;
else % BUG corrected, reshape L in row
    L = reshape(L,1,[]);
end
if nargin<7 || isempty(U)
    U=+inf(1,nvar);
elseif isscalar(U) % extend U if it's a scalar input
    Uval=U;
    U=zeros(1,nvar);
    U(:)=Uval;
else % BUG corrected, reshape U in row
    U = reshape(U,1,[]);
end

%
% Dimension check
%
if length(beq)~=neq || length(b)~=nle || ...
   length(cost)~=nvar || ...
   length(L)~=nvar || length(U)~=nvar || ...
   sizeA2~=nvar
    error('BuildMPS:DimensionsUnMatched', ...
          'BuildMPS: dimensions do not match');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Set problem name
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
l_name=setfields([],0,'NAME');
l_name=setfields(l_name,3,PbName);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Set equations in ROWS and COST
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
l_rows=setfields([],0,'ROWS');

l_cost=setfields([],1,'N',2,'COST');

l_rows_eq=emptyline(neq);
for m=1:neq
    l_rows_eq(m,:)=setfields(l_rows_eq(m,:),1,'E',2,eqtnames{m});
end

l_rows_le=emptyline(nle+nquadle);
for m=1:nle
    l_rows_le(m,:)=setfields(l_rows_le(m,:),1,'L',2,elenames{m});
end
for m=1:nquadle
    l_rows_le(nle+m,:)=setfields(l_rows_le(nle+m,:),1,'L',2,quadle(m).name);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Set coefficients of constraint equations in COLUMNS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Put all the linear constraint terms of quadratic in a matrix
quadle_g = cat(2,quadle(:).g).'; % (nquadle x nvar)
CostAeq = [cost(:).';
           Aeq;
           A;
           quadle_g]; % CostAeq is sparse if any is sparse
MustWrite = (CostAeq ~= 0);
NWrite = sum(MustWrite,1);
NLines = sum(ceil(NWrite/2));

l_columns=setfields([],0,'COLUMNS');
l_columnsbody=emptyline(NLines);

c=0;
for n=1:nvar % Loop over variables
    var=varnames{n};
    field=3;
    eqtn = find(MustWrite(:,n)); % subset of (1:1+neq+nle+nquadle)
    for m=eqtn(:).' % 1:1+neq+nle+nquadle % Loop over eqt
        if m==1
            colname='COST';
            val = cost(n);
        elseif m<=1+neq
            colname=eqtnames{m-1};
            val=Aeq(m-1,n);
        elseif m<=1+neq+nle
            colname=elenames{m-(1+neq)};
            val=A(m-(1+neq),n);
        else
            iquad = m-(1+neq+nle);
            colname=quadle(iquad).name;
            val=quadle_g(iquad,n);
        end
        if field==3
            c=c+1;
            l_columnsbody(c,:)=setfields(l_columnsbody(c,:),...
                2,var,...
                field,colname, ...
                field+1,val);
            field=5;
        else % field==5
            l_columnsbody(c,:)=setfields(l_columnsbody(c,:),...
                field,colname, ...
                field+1,val);
            field=3;
        end
    end % for-loop eqt
end % for-loop variables
l_columnsbody(c+1:end,:)=[];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Set equation RHS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
quadle_bquad = [quadle(:).bquad]; % (1 x nquadle)

rhs=[beq(:); b(:); quadle_bquad(:)];
MustWrite = (rhs ~= 0);
NWrite = sum(MustWrite);
NLines = ceil(NWrite/2);

l_rhs=setfields([],0,'RHS');
l_rhsbody=emptyline(NLines);
c=0;
field=3;
eqt = find(MustWrite); % subset of (1:neq+nle)
for m=eqt(:).' % 1:neq+nle+nquadle % Loop over eqt
    if m<=neq
        colname=eqtnames{m};
        val=rhs(m);
    elseif m<=neq+nle
        colname=elenames{m-neq};
        val=rhs(m);
    else
        colname=quadle(m-(neq+nle)).name;
        val=rhs(m);
    end
    if field==3
        c=c+1;
        l_rhsbody(c,:)=setfields(l_rhsbody(c,:),...
            2,'RHS',...
            field,colname, ...
            field+1,val);
        field=5;
    else
        l_rhsbody(c,:)=setfields(l_rhsbody(c,:),...
            field,colname, ...
            field+1,val);
        field=3;
    end
end % for-loop eqt
l_rhsbody(c+1:end,:)=[];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Set bound constraints
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
l_bound=setfields([],0,'BOUNDS');

VarType=zeros(size(U));
%
% Var types (local definition)
%
VarType(:)=0; % real
VarType(iset) = 1; % integer
VarType(bset) = 2; % binary

% Force lower/upper bound for integer variables to be integer as well
L(iset) = max(ceil(L(iset)),0); % integer lower bound cannot be negative
U(iset) = floor(U(iset));

% Values not used, but we set for clarity
L(bset) = 0;
U(bset) = 1;

upinf=(U==inf);
loinf=(L==-inf);
lonz=(L~=0) & ~loinf;

BoundType=zeros(size(U));

%
% Bound types (local definition)
%
BoundType(:) = 3; % Default, 0<=x, real variable
BoundType(upinf & loinf) = 1; % free, real
BoundType(upinf & lonz) = 2; % lo<=x (lo ~= 0), real
BoundType(~upinf & lonz) = 4; % lo<=x<=up, integer or real
BoundType(~upinf & loinf) = 5; % x<=up, real
BoundType(~upinf & ~loinf & ~lonz) = 6; % 0<=x<=up, integer or real
BoundType(upinf & VarType==1) = 7; %  lo<=x, integer
BoundType(bset) = 8; % binary, x = 0 or 1

NLines = sum(ismember(BoundType,[1 2 6 7 8])) + ...
         sum(ismember(BoundType,[4 5]))*2;
l_boundbody=emptyline(NLines);
c=0;
for n=1:nvar
    var=varnames{n};
    lo=L(n);
    up=U(n);
    vtype = VarType(n);
    if (vtype==2) % Type 8, binary variables
        c=c+1;
        l_boundbody(c,:)=setfields(l_boundbody(c,:),...
            1, 'BV', ...
            2, 'BND1', ...
            3, var, ...
            4, 1); % Field 4 must be 1.0 or blank
    elseif (up==inf)
        if (lo==-inf) % Type 1, Free real variable, one line
            c=c+1;
            l_boundbody(c,:)=setfields(l_boundbody(c,:),...
                1, 'FR', ...
                2, 'BND1', ...
                3, var, ...
                4, 0);
        elseif (lo~=0) || (vtype==1) % Type 2, or Type 7 lo<=x, one line
            c = c+1;
            if vtype==1 % integer, Type 7
                LOstr = 'LI';
            else % real, Type 2
                LOstr = 'LO';
            end
            l_boundbody(c,:)=setfields(l_boundbody(c,:),...
                1, LOstr, ...
                2, 'BND1', ...
                3, var, ...
                4, lo);
        % else 0<=x<=inf: Type3, real variable nothing to write
        end
    else % up<inf
        if lo>-inf
            if lo~=0 % Type 4, lo<=x<=up
                c=c+1;
                if vtype==1 % integer
                    LOstr = 'LI';
                else % real
                    LOstr = 'LO';
                end
                l_boundbody(c,:)=setfields(l_boundbody(c,:),...
                    1, LOstr, ...
                    2, 'BND1', ...
                    3, var, ...
                    4, lo);
            %else % 0<=x<=up % Type 6
            end
        else % if lo==-inf % Type 5, x<=up
            c=c+1;
            l_boundbody(c,:)=setfields(l_boundbody(c,:),...
                1, 'MI', ...
                2, 'BND1', ...
                3, var, ...
                4, 0);
        end
        % Common Type 4, 5, or 6
        % Type 6 is 0<=x<=up
        c=c+1;
        if vtype==1 % integer
            HIstr = 'UI';
        else % real
            HIstr = 'UP';
        end
        l_boundbody(c,:)=setfields(l_boundbody(c,:),...
            1, HIstr, ...
            2, 'BND1', ...
            3, var, ...
            4, up);
    end
end % for-loop on variable
l_boundbody(c+1:end,:)=[];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Quad section
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for m=1:nquadle
    % Get the current quad structure
    qs = quadle(m);
    l_quad = setfields([],0,'QSECTION');
    l_quad = setfields(l_quad,3,qs.name);

    % Use only the upper-triangular part of Q
    [i j Qij] = find(triu(qs.Q));
    NLines = length(Qij);
    l_quadbody = emptyline(NLines);

    for n=1:NLines % Loop over non-zeros elements
        vari=varnames{i(n)};
        varj=varnames{j(n)};
        l_quadbody(n,:) = setfields(l_quadbody(n,:), ...
            2,vari,...
            3,varj, ...
            4,Qij(n));
    end

    quadle(m).qsection = [l_quad;
                          l_quadbody]; %#ok

end % for-loop on quadratic constraints

if nquadle>1
    % concatenate together all the qsections
    l_allquad = cat(1,quadle(:).qsection);
else
    % empty line
    l_allquad = l_name([],:);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Set the last card
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
l_end=setfields([],0,'ENDATA');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Concatenate together all parts of mps format
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Contain=[l_name; ...
         l_rows; ...
         l_cost; ...
         l_rows_eq; ...
         l_rows_le; ...
         l_columns; ...
         l_columnsbody; ...
         l_rhs; ...
         l_rhsbody; ...
         l_bound; ...
         l_boundbody; ...
         l_allquad; ...
         l_end];

if ~isempty(MPSfilename)
    %
    % Save the Contain in MPSfilename
    %
    OK = SaveMPS(MPSfilename, Contain);
    if ~OK % Something is wrong during saving
        warning('BuildMPS:SavingFailure', ...
                ['BuildMPS: Cannot save ' MPSfilename]);
    end
else % Nothing to save
    OK = 1;
end

% return % Uncomment the RETURN statement causes M-lint to crash on 2009A
% There is no instructions from now on, juts nested functions

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Nested functions: BE AWARE, the functions have access to local
% variables of BuildMPS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   % Generate n empty lines of MPS data
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function l=emptyline(n)
        if nargin<1 || isempty(n)
            n=1;
        end
        l=char(zeros(n,61));
        l(:)=' ';
    end

   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   % Convert to string at the fixed length of 12
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function str=num2fixedlengthstr(num, maxlength, roundingflag)
        % function str=num2fixedlengthstr(num); OR
        % str=num2fixedlengthstr(..., maxlength, roundingflag);
        %
        % Convert double NUM to decimal string having MAXLENGTH [12] as maximum
        % length. Smart conversion with accurate result despite length constraint.
        %
        % ROUNDINGFLAG: 0 or [1]
        %   0: truncate fracional part (quicker)
        %   1: rounding fracional part (more accurate).
        %
        % Last update: 15/Aug/2008, remove leading "0" when the string starts
        %              as "0.xxxx"
        %
        if nargin<2
            maxlength=12;
        end

        if nargin<3
            roundingflag=1; % rounding by default
        end

        if num>=0
            fracNDigits=maxlength;
        else
            fracNDigits=maxlength-1;
        end
        % "%G" format:
        % ANSI specification X3.159-1989: "Programming Language C,"
        % ANSI, 1430 Broadway, New York, NY 10018.
        str=num2str(num,['%0.' num2str(fracNDigits) 'G']);
        %
        % Try to compact the string data to fit inside the field length
        %
        while length(str)>maxlength
            if regexp(str,'^0\.') % delete the leading 0 in "0.xxx"
                str(1)=[];
                continue;
            end
            [istart iend]=regexp(str,'[+-](0)+'); % +/- followed by multiples 0
            if ~isempty(istart) % Remove zero in xxxE+000yy or xxxE-000yy
                str(istart+1:iend)=[];
                continue
            else
                [istart iend]=regexp(str,'E[+]');
                if ~isempty(istart) % Remove "+" char in xxxE+yyy
                    str(iend)=[];
                    continue
                end
            end
            idot=find(str=='.',1,'first');
            if ~isempty(idot)
                iE=find(str=='E',1,'first');
                if roundingflag % rounding fraction part
                    % Calculate the Length of the fractional part
                    % Adjust its number of digits and start over again
                    if ~isempty(iE) % before the mantissa
                        fracNDigits=maxlength-length(str)+iE-idot-1;
                        str=num2str(num,['%0.' num2str(fracNDigits) 'E']);
                    else %if idot<=maxlength+1 % no manissa
                        fracNDigits=maxlength-idot;
                        str=num2str(num,['%0.' num2str(fracNDigits) 'f']);
                    end
                    roundingflag=0; % won't do rounding again
                    continue % second pass with new string
                else
                    % truncate the fractional part
                    if ~isempty(iE) % before the mantissa
                        str(maxlength-length(str)+iE:iE-1)=[];
                        return;
                    else %if idot<=maxlength+1 % no mantissa
                        str(maxlength+1:end)=[];
                        return;
                    end
                end
            end
            % it should not never go here, unless BUG
            error('BuildMPS: cannot convert %0.12e to string\n',num);
        end % while loop

    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Set the field of an MPS line by value
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function l=setfield(l,field,var)

        if isnumeric(var) % numerical data, convert to string
            var=num2fixedlengthstr(var); % convert to 12-length string
        end

        if isempty(l)
            l=emptyline;
        end
        if ~isempty(field) && field>0
            idx=idxlist{field};
        else
            idx=1:61;
        end
        if length(var)>length(idx)
            var=var(1:length(idx));
        else
            idx=idx(1:length(var));
        end
        l(idx)=var;

    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Set multiple fields of an MPS line by values
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function l=setfields(l, varargin)
        for k=1:2:length(varargin)
            l=setfield(l, varargin{k}, varargin{k+1});
        end
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Generate equation name for (LE) constraint
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function name=elename(m)
        name=['LE' num2str(m)];
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Generate equation name for (EQ) constraint
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function name=eqtname(m)
        name=['EQ' num2str(m)];
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Generate variable name
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function name=varname(n)
        name=['X' num2str(n)];
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Generate equation name for (QLE) constraint
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function name=quadlename(m)
        name=['QLE' num2str(m)];
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Parse a pair of Name/Value option
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function parseoption(strname, value)
        if ischar(strname)
            strname = strtrim(lower(strname));
            switch strname
                case 'elenames',
                    if ~iscell(value) || length(value)~=nle || ...
                        ~all(cellfun(@ischar, value))
                        error('BuildMPS:IncorrectEleNames', ...
                    'BuildMPS: EleNames must be cell of %d strings', nle);
                    end
                    elenames = value;
                case 'eqtnames',
                    if ~iscell(value) || length(value)~=neq || ...
                            ~all(cellfun(@ischar, value))
                        error('BuildMPS:IncorrectEqtNames', ...
                    'BuildMPS: EqtNames must be cell of %d strings', neq);
                    end
                    eqtnames = value;
                case 'varnames',
                    if ~iscell(value) || length(value)~=nvar || ...
                            ~all(cellfun(@ischar, value))
                        error('BuildMPS:IncorrectVarNames', ...
                    'BuildMPS: VarNames must be cell of %d strings', nvar);
                    end
                    varnames = value;
                case 'varnamefun',
                    if ischar(value)
                        value=str2func(value);
                    end
                    if ~isa(value,'function_handle')
                        error('BuildMPS:IncorrectVarNameFun', ...
                              'BuildMPS: VarNameFun must be a function');
                    end
                    varnamefun = value;
                case 'eqtnamefun',
                    if ischar(value)
                        value=str2func(value);
                    end
                    if ~isa(value,'function_handle')
                        error('BuildMPS:IncorrectEqtNameFun', ...
                              'BuildMPS: EqtNameFun must be a function');
                    end
                    eqtnamefun = value;
                case 'elenamefun',
                    if ischar(value)
                        value=str2func(value);
                    end
                    if ~isa(value,'function_handle')
                        error('BuildMPS:IncorrectEleNameFun', ...
                              'BuildMPS: EleNameFun must be a function');
                    end
                    elenamefun = value;
                case 'mpsfilename',
                    if ~ischar(value)
                        error('BuildMPS:IncorrectMPSfilename', ...
                              'BuildMPS: MPSfilename must be a string');
                    end
                    MPSfilename = value;
                case  {'i' 'int' 'integer' 'integers'},
                    iset = value(:);
                    if any(iset<1 | iset>nvar)
                        error('Integer set contains invalid index');
                    end
                case {'b' 'bin' 'binary' 'binaries'},
                    bset = value(:);
                    if any(bset<1 | bset>nvar)
                        error('Binary set contains invalid index');
                    end
                case {'quad' 'q'},
                    qcounter = length(quadle)+1;
                    % Basic check of quad structure
                    if isstruct(value)
                        qs = value;
                        if ~isfield(qs,'Q') || ~isequal(size(qs.Q),[nvar nvar])
                            error('Missing or invalid <Q> field in QUAD structure');
                        end
                        if ~isfield(qs,'g') || isempty(qs.g)
                            qs.g = zeros(nvar,1);
                        elseif isequal(size(qs.g), [1 nvar])
                            % reshape in column
                            qs.g = qs.g(:);
                        elseif ~isequal(size(qs.g), [nvar 1])
                            error('Invalid <g> field in QUAD');
                        end
                        if ~isfield(qs,'bquad') || isempty(qs.bquad)
                            qs.bquad = 0;
                        end
                        if ~isscalar(qs.bquad)
                            error('Missing or invalid <bquad> field in QUAD structure');
                        end
                        if ~isfield(qs,'type')
                            qs.type = 'QLE';
                        end
                        if ~strcmpi(qs.type ,'QLE')
                            error('Invalid <type> field in QUAD structure');
                        end
                        if ~isfield(qs,'name') || isempty(qs.name)
                            qs.name = quadlename(qcounter);
                        elseif strcmpi(qs.name,'COST') %
                            qs.name = 'COST'; % force to be upper case
                            % The linear term for the functional must be
                            % provides in the 5th parameter
                            % so we set 'g' to zero
                            qs.g(:) = 0;
                            qs.bquad(:) = 0;
                        end
                    else
                        if isempty(value)
                            return % ignore empty argument
                        end
                        error('Invalid input QUAD (must be a structure)');
                    end
                    quadle(qcounter) = orderfields(qs);
                otherwise
                    warning('BuildMPS:UnknownParams', ...
                        ['BuildMPS: Unknown parameter ' strname]);
            end
        else
            error('BuildMPS:IncorrectCall', ...
                  'BuildMPS: options must be pair of Name/Value');
        end
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Parse options
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function parseoptions(varargin)
        if mod(nargin,2)
            error('BuildMPS:IncorrectCall', ...
                  'BuildMPS: options must be pair of Name/Value');
        end

        % default empty quadle
        quadle = struct('Q', {}, ...
                        'g', {}, ...
                        'bquad', {}, ...
                        'type', {}, ...
                        'name', {} ...
                        );

        quadle = orderfields(quadle);

        %
        % Loop over pair of Name/Value option
        %
        for ivararg=1:2:nargin
            parseoption(varargin{ivararg},varargin{ivararg+1});
        end

    end

end % BuildMPS


clear, clc;

% This is an example for running the function LogisticR
%
%  Problem:
%
%  min  f(x,c) = - weight_i * log (p_i) + 1/2 * rsL2 * ||x||_2^2
%                 + rho * \|x\|_1
%
%  a_i denotes a training sample,
%      and a_i' corresponds to the i-th row of the data matrix A
%
%  y_i (either 1 or -1) is the response
%
%  p_i= 1/ (1+ exp(-y_i (x' * a_i + c) ) ) denotes the probability
%
%  weight_i denotes the weight for the i-th sample
%
% For detailed description of the function, please refer to the Manual.
%
%% Related papers
%
% [1]  Jun Liu and Jieping Ye, Efficient Euclidean Projections
%      in Linear Time, ICML 2009.
%
% [2]  Jun Liu and Jieping Ye, Sparse Learning with Efficient Euclidean
%      Projections onto the L1 Ball, Technical Report ASU, 2008.
%
% [3]  Jun Liu, Jianhui Chen, and Jieping Ye,
%      Large-Scale Sparse Logistic Regression, KDD, 2009.
%
%% ------------   History --------------------
%
% First version on August 10, 2009.
%
% September 5, 2009: adaptive line search is added
%
% For any problem, please contact Jun Liu (j.liu@asu.edu)

cd ..
cd ..

root=cd;
addpath(genpath([root '/SLEP']));
                     % add the functions in the folder SLEP to the path

% change to the original folder
cd Examples/L1;

m=1000;  n=1000;     % The data matrix is of size m x n

randNum=2;           % a random number

% ---------------------- generate random data ----------------------
randn('state',(randNum-1)*3+1);
A=randn(m,n);        % the data matrix

randn('state',(randNum-1)*3+2);
xOrin=randn(n,1);

randn('state',(randNum-1)*3+3);
y=[ones(n/2,1);...
    -ones(n/2, 1)];  % the response

rho=0.1;             % the regularization parameter
                     % it is a ratio between (0,1), if .rFlag=1

%----------------------- Set optional items -----------------------
opts=[];

% Starting point
opts.init=2;        % starting from a zero point

% Termination
opts.tFlag=5;       % run .maxIter iterations
opts.maxIter=100;    % maximum number of iterations

% Normalization
opts.nFlag=0;       % without normalization

% Regularization
opts.rFlag=1;       % the input parameter 'rho' is a ratio in (0, 1)
%opts.rsL2=0.01;     % the squared two norm term

% Group Property
opts.sWeight=[1,1]; % set the weight for positive and negative samples

%----------------------- Run the code LeastR -----------------------
fprintf('\n mFlag=0, lFlag=0 \n');
opts.mFlag=0;       % treating it as compositive function
opts.lFlag=0;       % Nemirovski's line search
tic;
[x1, c1, funVal1, ValueL1]= LogisticR(A, y, rho, opts);
toc;

opts.maxIter=1000;

fprintf('\n mFlag=1, lFlag=0 \n');
opts.mFlag=1;       % smooth reformulation
opts.lFlag=0;       % Nemirovski's line search
opts.tFlag=2; opts.tol= funVal1(end);
tic;
[x2, c2, funVal2, ValueL2]= LogisticR(A, y, rho, opts);
toc;

fprintf('\n mFlag=1, lFlag=1 \n');
opts.mFlag=1;       % smooth reformulation
opts.lFlag=1;       % adaptive line search
opts.tFlag=2; opts.tol= funVal1(end);
tic;
[x3, c3, funVal3, ValueL3]= LogisticR(A, y, rho, opts);
toc;

figure;
plot(funVal1,'-r');
hold on;
plot(funVal2,'--b');
hold on;
plot(funVal3,':g');
legend('mFlag=0, lFlag=0', 'mFlag=1, lFlag=0', 'mFlag=1, lFlag=1');
xlabel('Iteration (i)');
ylabel('The objective function value');

% --------------------- compute the pathwise solutions ----------------
opts.fName='LogisticR';    % set the function name to 'LogisticR'
Z=[0.5, 0.2, 0.1, 0.01];   % set the parameters

% run the function pathSolutionLogistic
fprintf('\n Compute the pathwise solutions, please wait...');
[X,C]=pathSolutionLogistic(A, y, Z, opts);


 function [fw, hr, hl] = fwhm(psfs, ii, doplot)
%function [fw, hr, hl] = fwhm(psfs, ii, doplot)
% compute fwhm of point-spread function centered at pixel ii
% and half-right width and half-left width (fw = hr + hl)

if ~nargin, ir_usage, end

warning 'fwhm is obsolete.  use fwhm1 for 1D fwhm'

[np nc] = size(psfs);
if (np == 1)
	psfs = psfs';
	[np, nc] = size(psfs);
end

warned = false;
for ic = 1:nc
	psf = psfs(:,ic);
	if (nargin < 2)
		ii = imax(psf);
	end

	% normalize
	psf = psf / psf(ii);
	if ~warned && (1 ~= max(psf))
		warning('peak not at center')
		warned = true;
	end

	% right
	ir = sum(cumprod(double6(psf((ii+1):np) >= 0.5)));
	high	= psf(ii + ir);
	low	= psf(ii + ir + 1);
	hr(ic,1) = ir + (high - 1/2) / (high-low);

	% left
	il = sum(cumprod(double6(psf((ii-1):-1:1) >= 0.5)));
	high	= psf(ii - il);
	low	= psf(ii - il - 1);
	hl(ic,1) = il + (high - 1/2) / (high-low);
end

fw = hr + hl;

if nargin > 2
	plot(1:np, psf, 'o', ii+[-left right], [0.5 0.5], '-')
end


function pass = test_biharm( )

tol = 1e4*chebfunpref().cheb2Prefs.chebfun2eps;

%  Test some spherical harmonics
k = 1;
for ell = [1 2 4 5]
    for m = 0:ell
        f = spherefun.sphharm(ell, m);
        lap2 = biharm(f);
        pass(k, 1) = numel(lap2.pivotValues) == numel(f.pivotValues);
        err(k) = SampleError((ell*(ell+1))^2*f, lap2)/(ell*(ell+1))^2;
        pass(k, 2) = SampleError((ell*(ell+1))^2*f, lap2) < (ell*(ell+1))^2*tol;
        k = k+1;
    end
end
pass = pass(:)';

end

function sample_error = SampleError(h, g)
m = 6;
n = m;
[x, y] = getPoints(m, n);
[L2, T2] = meshgrid(x, y);
F = feval(h, L2, T2);
approx = fevalm(g, x, y);
sample_error = norm(F(:) - approx(:), inf);
end

function [x, y] = getPoints(m, n)

x = trigpts(2*n, [-pi pi]);
y = linspace(0, pi, m).';

end


%--- help for generic/is_stable_system ---
%
%  Checks the stability of a linear markov switching system.
%
%  ::
%
%     flag = is_stable_system(obj)
%     flag = is_stable_system(obj,varargin)
%
%  Args:
%
%     obj (dsge | rise | svar | rfvar): model object
%
%     varargin (name,value): pairwise valid options for RISE. The most
%       relevant in this case are
%
%        - **stability_criterion** [numeric\|{1.000001}]: stability criterion.
%          All eigenvalues must be smaller than this criterion for the system to
%          be MSS
%        - **stability_algorithm** ['cfm'\|{'hmg'}]: CFM stands for
%          Costa-Fragoso-Marques while HMG stands for Hassibi-Murray-Gupta.
%
%  Returns:
%     :
%
%     - **flag** [false\|true]: result of the investigation on whether the
%       system is stable or not.
%
%  Note:
%
%     RISE implements two algorithms from the engineering literature to check
%     for the stability. They are
%
%        - Costa-Fragoso-Marques :cite:`costa2006discrete`
%        - Hassibi-Murray-Gupta :cite:`gupta2003control`
%
%     Refer to the references to see the specific algorithms. However, for most
%     applications, one can just use the default options.
%
%  References:
%
%     - :cite:`costa2006discrete`
%     - :cite:`gupta2003control`
%
%


function [mappedX, mapping] = kernel_pca(X, no_dims, varargin)
%KERNEL_PCA Perform the kernel PCA algorithm
%
%   [mappedX, mapping] = kernel_pca(X, no_dims)
%   [mappedX, mapping] = kernel_pca(X, no_dims, kernel)
%   [mappedX, mapping] = kernel_pca(X, no_dims, kernel, param1)
%   [mappedX, mapping] = kernel_pca(X, no_dims, kernel, param1, param2)
%
% The function runs kernel PCA on a set of datapoints X. The variable
% no_dims sets the number of dimensions of the feature points in the
% embedded feature space (no_dims >= 1, default = 2).
% For no_dims, you can also specify a number between 0 and 1, determining
% the amount of variance you want to retain in the PCA step.
% The value of kernel determines the used kernel. Possible values are 'linear',
% 'gauss', 'poly', 'subsets', or 'princ_angles' (default = 'gauss'). For
% more info on setting the parameters of the kernel function, type HELP
% GRAM.
% The function returns the locations of the embedded trainingdata in
% mappedX. Furthermore, it returns information on the mapping in mapping.
%
%

% This file is part of the Matlab Toolbox for Dimensionality Reduction.
% The toolbox can be obtained from http://homepage.tudelft.nl/19j49
% You are free to use, change, or redistribute this code in any way you
% want for non-commercial purposes. However, it is appreciated if you
% maintain the name of the original author.
%
% (C) Laurens van der Maaten, Delft University of Technology

    if ~exist('no_dims', 'var')
        no_dims = 2;
    end
    kernel = 'gauss';
    param1 = 1;
	param2 = 3;
    if nargin > 2
		kernel = varargin{1};
		if length(varargin) > 1 & strcmp(class(varargin{2}), 'double'), param1 = varargin{2}; end
		if length(varargin) > 2 & strcmp(class(varargin{3}), 'double'), param2 = varargin{3}; end
    end

    % Store the number of training and test points
    ell = size(X, 1);

    if size(X, 1) < 2000

        % Compute Gram matrix for training points
        disp('Computing kernel matrix...');
        K = gram(X, X, kernel, param1, param2);

        % Normalize kernel matrix K
        mapping.column_sums = sum(K) / ell;                       % column sums
        mapping.total_sum   = sum(mapping.column_sums) / ell;     % total sum
        J = ones(ell, 1) * mapping.column_sums;                   % column sums (in matrix)
        K = K - J - J';
        K = K + mapping.total_sum;

        % Compute first no_dims eigenvectors and store these in V, store corresponding eigenvalues in L
        disp('Eigenanalysis of kernel matrix...');
        K(isnan(K)) = 0;
        K(isinf(K)) = 0;
        [V, L] = eig(K);
    else
        % Compute column sums (for out-of-sample extension)
        mapping.column_sums = kernel_function([], X', 1, kernel, param1, param2, 'ColumnSums') / ell;
        mapping.total_sum   = sum(mapping.column_sums) / ell;

        % Perform eigenanalysis of kernel matrix without explicitly
        % computing it
        disp('Eigenanalysis of kernel matrix (using slower but memory-conservative implementation)...');
        options.disp = 0;
        options.isreal = 1;
        options.issym = 1;
        [V, L] = eigs(@(v)kernel_function(v, X', 1, kernel, param1, param2, 'Normal'), size(X, 1), no_dims, 'LM', options);
        disp(' ');
    end

    % Sort eigenvalues and eigenvectors in descending order
    [L, ind] = sort(diag(L), 'descend');
    L = L(1:no_dims);
	V = V(:,ind(1:no_dims));

    % Compute inverse of eigenvalues matrix L
	disp('Computing final embedding...');
    invL = diag(1 ./ L);

    % Compute square root of eigenvalues matrix L
    sqrtL = diag(sqrt(L));

    % Compute inverse of square root of eigenvalues matrix L
    invsqrtL = diag(1 ./ diag(sqrtL));

    % Compute the new embedded points for both K and Ktest-data
    mappedX = sqrtL * V';                     % = invsqrtL * V'* K

    % Set feature vectors in original format
    mappedX = mappedX';

    % Store information for out-of-sample extension
    mapping.X = X;
    mapping.V = V;
    mapping.invsqrtL = invsqrtL;
    mapping.kernel = kernel;
    mapping.param1 = param1;
    mapping.param2 = param2;


classdef CEC2017_F5 < PROBLEM
% <single> <real> <constrained>
% CEC'2017 constrained optimization benchmark problem

%------------------------------- Reference --------------------------------
% G. Wu, R. Mallipeddi, and P. N. Suganthan, Problem definitions and
% evaluation criteria for the CEC 2017 competition on constrained real-
% parameter optimization, National University of Defense Technology, China,
% 2016.
%------------------------------- 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".
%--------------------------------------------------------------------------

    properties
        O;      % Optimal decision vector
        Mat1;	% Rotation matrices
        Mat2;
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            CallStack = dbstack('-completenames');
            load(fullfile(fileparts(CallStack(1).file),'CEC2017.mat'),'Data');
            obj.O = Data{5}.o;
            obj.M = 1;
            if isempty(obj.D) || obj.D < 30
                obj.D    = 10;
                obj.Mat1 = Data{5}.M1_10;
                obj.Mat2 = Data{5}.M2_10;
            elseif obj.D < 50
                obj.D    = 30;
                obj.Mat1 = Data{5}.M1_30;
                obj.Mat2 = Data{5}.M2_30;
            elseif obj.D < 100
                obj.D    = 50;
                obj.Mat1 = Data{5}.M1_50;
                obj.Mat2 = Data{5}.M2_50;
            else
                obj.D    = 100;
                obj.Mat1 = Data{5}.M1_100;
                obj.Mat2 = Data{5}.M2_100;
            end
            obj.lower    = zeros(1,obj.D) - 10;
            obj.upper    = zeros(1,obj.D) + 10;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            Z = PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            PopObj = sum(100*(Z(:,1:end-1).^2-Z(:,2:end)).^2+(Z(:,1:end-1)-1).^2,2);
        end
        %% Calculate constraint violations
        function PopCon = CalCon(obj,PopDec)
            Z = PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            Y = Z*obj.Mat1';
            W = Z*obj.Mat2';
            PopCon(:,1) = sum(Y.^2-50*cos(2*pi*Y)-40,2);
            PopCon(:,2) = sum(W.^2-50*cos(2*pi*W)-40,2);
        end
    end
end


function [afixed,sqnorm,ierr] = lsearch (afloat,L,D,Chi2,ncands)
%LSEARCH: Integer ambiguity resolution, search
%
% This routine finds the integer vector which is closest to a given
% float vector, in a least squares sence. This is the search-step in
% integer ambiguity resolution. It is best to perform this search only
% on ambiguities which have been decorrelated using LAMBDA.
%
% Input arguments:
%    afloat : Float ambiguities (\hat{a})
%    L      : LtDL-decomposition of the decorrelated
%    D        variance-covariance matrix of the ambiguities
%    Chi2   : Size of the search ellipsoid
%    ncands : Number of requested candidates
%
% Output arguments:
%    afixed : Estimated integers (matrix)
%    sqnorm : Corresponding squared norms (vector, sorted)
%    ierr   : Error code: 0: No errors found
%                         1: Not enough candidates found

% ----------------------------------------------------------------------
% File.....: lsearch.m
% Date.....: 19-MAY-1999
% Author...: Peter Joosten
%            Mathematical Geodesy and Positioning
%            Delft University of Technology
% ----------------------------------------------------------------------

% -------------------------------
% --- Initializing statements ---
% -------------------------------

Linv      = (L)^(-1);
Dinv      = 1./D;

True      = 1;
False     = 0;

n         = max(size(afloat));

right     = [zeros(n,1) ; Chi2];
left      = [zeros(n+1,1)];
dq        = [Dinv(2:n)./Dinv(1:n-1) 1/Dinv(n)];

cand_n    = False;
c_stop    = False;
endsearch = False;

ncan      = 0;

i         = n + 1;
iold      = i;
ierr      = 0;

afixed = zeros(n,ncands);
sqnorm = zeros(1,ncands);

% ----------------------------------
% --- Start the main search-loop ---
% ----------------------------------

while ~ (endsearch);

   i = i - 1;

   if iold <= i
      lef(i) = lef(i) + Linv(i+1,i);
   else
      lef(i) = 0;
      for j = i+1:n;
         lef(i) = lef(i) + Linv(j,i)*distl(j,1);
      end;
   end;
   iold = i;

   right(i)   = (right(i+1) - left(i+1)) * dq(i);
   reach      = sqrt(right(i));
   delta      = afloat(i) - reach - lef(i);
   distl(i,1) = ceil(delta) - afloat(i);

   if distl(i,1) > reach - lef(i)

%     ----------------------------------------------------
%     --- There is nothing at this level, so backtrack ---
%     ----------------------------------------------------

      cand_n = False;
      c_stop = False;

      while (~ c_stop) & (i < n);

         i = i + 1;
         if distl(i) < endd(i);
            distl(i) = distl(i) + 1;
            left(i)  = (distl(i) + lef(i)) ^ 2;
            c_stop   = True;
            if i == n; cand_n = True; end;
         end;

      end;

      if (i == n) & (~ cand_n); endsearch = True; end;

   else

%     ----------------------------
%     --- Set the right border ---
%     ----------------------------

      endd(i) = reach - lef(i) - 1;
      left(i) = (distl(i,1) + lef(i)) ^ 2;

   end

   if i == 1;

%     -------------------------------------------------------------------
%     --- Collect the integer vectors and corresponding               ---
%     --- squared distances, add to vectors "afixed" and "sqnorm" if: ---                             ---
%     --- * Less then "ncands" candidates found so far                ---
%     --- * The squared norm is smaller than one of the previous ones ---
%     -------------------------------------------------------------------

      t       = Chi2 - (right(1)-left(1)) * Dinv(1);
      endd(1) = endd(1) + 1;

      while distl(1) <= endd(1);

         if ncan < ncands;

            ncan             = ncan + 1;
            afixed(1:n,ncan) = distl + afloat;
            sqnorm(ncan)     = t;

         else

            [maxnorm,ipos] = max(sqnorm);
            if t < maxnorm;
               afixed(1:n,ipos) = distl + afloat;
               sqnorm(ipos)     = t;
            end;

         end;

         t       = t + (2 * (distl(1) + lef(1)) + 1) * Dinv(1);
         distl(1) = distl(1) + 1;

      end;

%     -------------------------
%     --- And backtrack ... ---
%     -------------------------

      cand_n = False;
      c_stop = False;

      while (~ c_stop) & (i < n);

         i = i + 1;

         if distl(i) < endd(i);
            distl(i) = distl(i) + 1;
            left(i)  = (distl(i) + lef(i)) ^ 2;
            c_stop   = True;
            if i == n; cand_n = True; end;
         end;

      end;

      if (i == n) & (~ cand_n); endsearch = True; end;

   end;

end;

% ----------------------------------------------------------------------
% --- Sort the resulting candidates, according to the norm
% ----------------------------------------------------------------------

tmp    = sortrows ([sqnorm' afixed']);
sqnorm = tmp(:,1)';
afixed = tmp(:,2:n+1)';

% ------------------------
% --- Check for errors ---
% ------------------------

if ncan < ncands; ierr = 1; end;

% ----------------------------------------------------------------------
% End of routine: lsearch
% ----------------------------------------------------------------------


% Feature sign search
% code by Wang Jinjun @ NEC Research Lab America
% reference
% Efficient sparse coding algorithms
%   Honglak Lee Alexis Battle Rajat Raina Andrew Y. Ng
%       Computer Science Department
%       Stanford University
%       Stanford, CA 94305

function [x]=feature_sign(B,y,lambda,init_x)

nbases=size(B,2);

OptTol = 1e-5;

if nargin < 4,
    x=zeros(nbases, 1);
else
    x = init_x;
end;

theta=sign(x);          %sign flag
a=(x~=0);               %active set

optc=0;

By=B'*y;
B_h=B(:,a);
x_h=x(a);
Bx_h=B_h*x_h;
all_d=2*(B'*Bx_h-By);
[ma mi]=max(abs(all_d).*(~a));

while optc==0,

    optc=1;

    if all_d(mi)>lambda+1e-10,
        theta(mi)=-1;
        a(mi)=1;
        b=B(:,mi);
        x(mi)=(lambda-all_d(mi))/(b'*b*2);
    elseif all_d(mi)<-lambda-1e-10,
        theta(mi)=1;
        a(mi)=1;
        b=B(:,mi);
        x(mi)=(-lambda-all_d(mi))/(b'*b*2);
    else
        if sum(a)==0,
            lambda=ma-2*1e-10;
            optc=0;
            b=B(:,mi);
            x(mi)=By(mi)/(b'*b);
            break;
        end
    end

    opts=0;
    B_h=B(:,a);
    x_h=x(a);
    theta_h=theta(a);

    while opts==0,
        opts=1;

        if size(B_h,2)<=length(y),
            BB=B_h'*B_h;
            x_new=BB\(B_h'*y-lambda*theta_h/2);
            o_new=L1_cost(y,B_h,x_new,lambda);

            %cost based on changing sign
            s=find(sign(x_new)~=theta_h);
            x_min=x_new;
            o_min=o_new;
            for j=1:length(s),
                zd=s(j);
                x_s=x_h-x_h(zd)*(x_new-x_h)/(x_new(zd)-x_h(zd));
                x_s(zd)=0;  %make sure it's zero
                o_s=L1_cost(y,B_h,x_s,lambda);
                if o_s<o_min,
                    x_min=x_s;
                    o_min=o_s;
                end
            end
        else
            d=x_h-B_h'*((B_h*B_h')\(B_h*x_h));
            q=x_h./(d+eps);
            x_min=x_h;
            o_min=L1_cost(y,B_h,x_h,lambda);
            for j=1:length(q),
                zd=q(j);
                x_s=x_h-zd*d;
                x_s(j)=0;       %make sure it's zero
                o_s=L1_cost(y,B_h,x_s,lambda);
                if o_s<o_min,
                   x_min=x_s;
                   o_min=o_s;
                end
            end
        end

        x(a)=x_min;

        a=(x~=0);
        theta=sign(x);

        B_h=B(:,a);
        x_h=x(a);
        theta_h=theta(a);
        Bx_h=B_h*x_h;

        active_d=2*(B_h'*(Bx_h-y))+lambda*theta_h;

        if ~isempty(find(abs(active_d)>OptTol)),
            opts=0;
        end
    end

    all_d=2*(B'*Bx_h-By);

    [ma mi]=max(abs(all_d).*(~a));
    if ma>lambda+OptTol,
        optc=0;
    end
end

return;

function cost=L1_cost(y,B,x,lambda)
    tmp = y-B*x;
    cost = tmp'*tmp+lambda*norm(x,1);
return


function upsilonjv = lfmjvComputeUpsilonMatrix(gamma, sigma2, t1, t2, mode)

% LFMJVCOMPUTEUPSILONMATRIX Upsilon matrix jolt. vel. with t1, t2 limits
% FORMAT
% DESC computes a portion of the LFMJV kernel.
% ARG gamma : Gamma value for system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG mode : operation mode, according to the derivative (mode 0,
% derivative wrt t1, mode 1 derivative wrt t2)
% RETURN upsilon : result of this subcomponent of the kernel for the given values.
%
% COPYRIGHT : Mauricio Alvarez, 2010
%
% SEEALSO : lfmComputeUpsilonMatrix.F, lfmvpComputeUpsilonMatrix.m

% KERN

sigma = sqrt(sigma2);
gridt1 = repmat(t1, 1, length(t2));
gridt2 = repmat(t2', length(t1), 1);
timeGrid = gridt1 - gridt2;

if mode==0
    upsilonjv = gamma^2*lfmvvComputeUpsilonMatrix(gamma, sigma2, t1, t2, mode) ...
        - (4/(sqrt(pi)*sigma^3))*exp(-(timeGrid.^2)./sigma2).* ...
        ((gamma + (2*timeGrid)/sigma2).*(1-(2*timeGrid.^2)/sigma2) + 4*timeGrid/sigma2);
else
    upsilonjv = gamma^2*lfmvvComputeUpsilonMatrix(gamma, sigma2, t1, t2, mode) ...
        - (4/(sqrt(pi)*sigma^3))*exp(-(timeGrid.^2)./sigma2).* ...
        ((gamma + (2*timeGrid)/sigma2).*(1-(2*timeGrid.^2)/sigma2) + 4*timeGrid/sigma2) ...
        + ((4*gamma)/(sqrt(pi)*sigma^3))*exp(-gamma*t1)*...
        ((t2.*(gamma - 2*t2/sigma2) + 1).*exp(-t2.^2/sigma2)).';
end


function [l,L_f,L_lf] = fromFrameIdpLin(F,lf)

% FROMFRAMEIDPLIN  Transforms IDP line from local frame to global frame.
%   I = FROMFRAMEIDPLIN(F,LF) transforms the Inverse Depth line LF from the
%   local frame F to the global frame. The frame F must be specified via a
%   structure containing at least the fields F.t, F.q, F.R and F.Rt
%   (translation, quaternion, rotation matrix and its transpose).
%
%   [I,L_f,L_lf] = FROMFRAMEIDPLIN(...) returns the Jacobians wrt F and IF.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

if nargout == 1
    [p1f,p2f] = idpLin2idpPnts(lf);

    p1 = fromFrameIdp(F,p1f);
    p2 = fromFrameIdp(F,p2f);

    l  = [p1;p2(4:6,:)];

else
    % idp parts
    xf  = lf(1:3);
    w1f = lf(4:5);
    r1  = lf(6);
    w2f = lf(7:8);
    r2  = lf(9);

    % dir. vectors
    [m1f, M1F_w1f] = py2vec(w1f);
    [m2f, M2F_w2f] = py2vec(w2f);

    % from frame
    [x, X_f, X_xf] = fromFrame(F,xf);
    [m1, M1_f, M1_m1f]   = fromFrameVec(F,m1f);
    [m2, M2_f, M2_m2f]   = fromFrameVec(F,m2f);

    % angle vectors
    [w1, W1_m1] = vec2py(m1);
    [w2, W2_m2] = vec2py(m2);

    % partial Jacobians
    W1_f   = W1_m1*M1_f;
    W2_f   = W2_m2*M2_f;
    W1_w1f = W1_m1*M1_m1f*M1F_w1f;
    W2_w2f = W2_m2*M2_m2f*M2F_w2f;
    R_f    = zeros(1,7);

    % new idp line
    l = [x;w1;r1;w2;r2];

    % Jacobians
    L_f  = [X_f;W1_f;R_f;W2_f;R_f];
    L_lf = [...
        X_xf        zeros(3)          zeros(3)
        zeros(2,3)  W1_w1f     [0;0]  zeros(2,3)
        0 0 0       0 0         1     0 0        0
        zeros(2,3)  zeros(2,3)        W2_w2f    [0;0]
        0 0 0       0 0         0     0 0        1    ] ;

end

return

%% jac

syms x y z a b c d X Y Z A1 B1 R1 A2 B2 R2 real
F.x = [x;y;z;a;b;c;d];
F   = updateFrame(F);
l_F = [X;Y;Z;A1;B1;R1;A2;B2;R2];

[l,L_f,L_lf] = fromFrameIdpLin(F,l_F);

simplify(L_f  - jacobian(l,F.x))
simplify(L_lf - jacobian(l,l_F))

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function sqi = psqi(signal,fs,btest,btot)
%pSQI Power of QRS SQI
%
% Returns the relative power on band P(5-20Hz)/P(5-45Hz). Operates in 1D or
% 2D vectors for some speeding up.
%
% Reference:
% Li, Q., Mark, R. G., Clifford, G. D., & Li. (2008). Robust heart rate
% estimation from multiple asynchronous noisy sources using signal quality
% indices and a Kalman filter. Physiol. Meas., 29(1), 15–32.
% http://doi.org/10.1088/0967-3334/29/1/002
%
% Input:
%   signal:         single channel (F)ECG [1xN double]
%
% Output:
%   sqi:            resulting sSQI for segment
%
% Fetal Extraction Toolbox, version 1.0, February 2014
% Released under the GNU General Public License
%
% Copyright (C) 2014 Fernando Andreotti
% Dresden University of Technology, Institute of Biomedical Engineering
% fernando.andreotti@mailbox.tu-dresden.de
%
% Last updated : 09-03-2014
%
%
% This program is free software; you can redistribute it and/or modify it
% under the terms of the GNU General Public License as published by the
% Free Software Foundation; either version 2 of the License, or (at your
% option) any later version.
% This program is distributed in the hope that it will be useful, but
% WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
% Public License for more details.

signal(isnan(signal)) = 0;

xdft = fft(detrend(signal),[],1);
xdft = xdft(1:floor(size(signal,1)/2+1),:);
xdft(2:end-1,:) = 2*xdft(2:end-1,:);
psdest = 1/(size(signal,1)*fs)*abs(xdft).^2;
freq = 0:fs/size(signal,1):fs/2;

% plot(freq,psdest);
% xlabel('Hz');
% grid on;
% title('Single-Sided Amplitude Spectrum of S(t)')
% xlabel('f (Hz)')
% ylabel('|P1(f)|')

if nargin < 3
    btest = [5 15];
    btot = [5 40];
end

pband = sum(psdest(freq>=btest(1)&freq<=btest(2),:));
ptot = sum(psdest(freq>=btot(1)&freq<=btot(2),:));
sqi = 1-(pband./ptot);

end


%DEMO_SURVIVAL_COMPARISON  Survival model comparison
%
%  Description:
%
%    By using kfc-validation and Bayesian bootstrap we compare the
%    predictive ability of different models by estimating various
%    assessment statistics .
%
%    We will compare two Cox proportional hazars model, the first
%    model will have less covariates than the second model.
%
%    The censoring indicator ye is
%
%      ye = 0 for uncensored event
%      ye = 1 for right censored event.
%
%    Example data set is leukemia survival data in Northwest
%    England presented in (Henderson, R., Shimakura, S., and Gorst,
%    D. (2002). Modeling spatial variation in leukemia survival
%    data. Journal of the American Statistical Association,
%    97:965–972). Data set was downloaded from
%    http://www.math.ntnu.no/%7Ehrue/r-inla.org/examples/leukemia/leuk.dat
%
%  See also  DEMO_SURVIVAL_COMPARISON2, DEMO_SURVIVAL_COXPH

% Copyright (C) 2012 Ernesto Ulloa, Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

%% First load data
S = which('demo_survival_weibull');
L = strrep(S,'demo_survival_weibull.m','demodata/leukemia.txt');
leukemiadata=load(L);

% leukemiadata consists of:
% 'time', 'cens', 'xcoord', 'ycoord', 'age', 'sex', 'wbc', 'tpi', 'district'

% survival times
y=leukemiadata(:,1);
% scale survival times
y=y/max(y);

ye=1-leukemiadata(:,2); % event indicator, ye = 0 for uncensored event
                        %                        ye = 1 for right censored event

%  we choose for the first model: 'age' and 'sex'covariates
x01=leukemiadata(:,5:6);
x1=x01;

%  we choose for the second model: 'age', 'sex', 'wbc', and 'tpi' covariates
x02=leukemiadata(:,5:8);
x2=x02;

% normalize continuous covariates

x1(:,1)=normdata(x01(:,1));
x2(:,[1 3:4])=normdata(x02(:,[1 3:4]));

[n1, nin1]=size(x1);
[n2, nin2]=size(x2);

% number of time intervals
ntime=50;
% create finite partition of time axis
S=linspace(0,max(y)+0.001,ntime+1);

%% obtain predictions

% Create the covariance functions
pl = prior_t('s2',1, 'nu', 4);
pm = prior_t('s2',1, 'nu', 4);

% covariance for hazard function
gpcfh1 = gpcf_sexp('lengthScale', 1, 'magnSigma2', 1.1, 'lengthScale_prior', pl, 'magnSigma2_prior', pm);
gpcfh2 = gpcf_sexp('lengthScale', 1, 'magnSigma2', 1.1, 'lengthScale_prior', pl, 'magnSigma2_prior', pm);

% covariance for proportional part
gpcf1 = gpcf_sexp('lengthScale', ones(1,size(x1,2)), 'magnSigma2', 1.2, 'lengthScale_prior', pl, 'magnSigma2_prior', pm);
gpcf2 = gpcf_sexp('lengthScale', ones(1,size(x2,2)), 'magnSigma2', 1.2, 'lengthScale_prior', pl, 'magnSigma2_prior', pm);

% Create the likelihood structure
lik = lik_coxph('S', S);

gp1 = gp_set('lik', lik, 'cf', {gpcfh1 gpcf1}, 'jitterSigma2', 1e-6, 'comp_cf', {[1] [2]});
gp2 = gp_set('lik', lik, 'cf', {gpcfh2 gpcf2}, 'jitterSigma2', 1e-6, 'comp_cf', {[1] [2]});

% Set the approximate inference method to Laplace
gp1 = gp_set(gp1, 'latent_method', 'Laplace');
gp2 = gp_set(gp2, 'latent_method', 'Laplace');

opt=optimset('TolFun',1e-2,'TolX',1e-4,'Display','iter','Derivativecheck','off');

% obtain predictions for both models using kfc-validation

%* first we set tau
tt=0.1:.1:1;

% set D event indicator vector for each time in tt (Di=0 if i experienced
% the event before tau and Di=1 otherwise)
% Also we set YY, the observed time vector for each time value in tt
for i=1:size(tt,2)
  for i2=1:size(ye,1)
    if y(i2)>tt(i)
      yytemp(i2)=tt(i);
      Dtemp(i2)=1;
    else
      if ye(i2)==1
        Dtemp(i2)=1;
      else
        Dtemp(i2)=0;
      end
      yytemp(i2)=y(i2);
    end
  end
  yyi{i}=yytemp';
  Di{i}=Dtemp';
end
for i=1:size(Di,2)
  D(:,i)=Di{i};
end
for i=1:size(yyi,2)
  yy(:,i)=yyi{i};
end

% set time vector to make predictions
yt=bsxfun(@times,ones(size(y)),tt);

% Obtain predictions
% (This takes several minutes)
crit1=gp_kfcv_cdf(gp1,x1,y,'z',D,'yt',yt,'opt',opt);
crit2=gp_kfcv_cdf(gp2,x2,y,'z',D,'yt',yt,'opt',opt);

%% Calculate statics and compare models
% MODEL COMPARISON

%% AUC
% AUC for Binary outcomes P(Pi>Pj | Di=1,Dj=0)
[auc1,fps1,tps1]=aucs(crit1(:,length(tt)),D(:,length(tt)));
[auc2,fps2,tps2]=aucs(crit2(:,length(tt)),D(:,length(tt)));

fprintf('AUC at end of study for model 1: %.3f \n', auc1);
fprintf('AUC at end of study for model 2: %.3f \n', auc2);
hold on
plot(fps1,tps1,'b')
plot(fps2,tps2,'r')
title('ROC curve')
legend('model 1', 'model 2',4)
xlabel('False positives')
ylabel('True positives')
hold off

%% Harrell's C
% Obtain for both models Binary AUC(t) = P(Pi>Pj | Di(t)=1,Dj(t)=0) and
% Harrell's C(t) = P(Pi>Pj | Di(ti)=1, ti<tj, ti<tt) for every element of tt
ct1=hct(crit1,yy,D,tt);
ct2=hct(crit2,yy,D,tt);
auct1=auct(crit1,yy,D,tt);
auct2=auct(crit2,yy,D,tt);
c=[ct1 ct2];

% Plot for both models Harrells C in function of time
plot(tt,c(:,1),'r');
hold on;
plot(tt,c(:,2),'g');
legend('Old model','New model')
title('Harrolds C in function of time ');
xlabel('Time');
ylabel('Harrell''s C');
hold off;

%% Estimated density
% Use bayesian bootsrap to obtain Harrells (C1-C2) statistic density at tt=1
[c1,bb1]=hcs(crit1(:,end),y,ye,1,'rsubstream',1);
[c2,bb2]=hcs(crit2(:,end),y,ye,1,'rsubstream',1);
title('Estimated density of C2-C1')
lgpdens(bb2-bb1)
xlabel('Difference in Harrell''s C statistics (C2-C1)');

% We integrate the (C1-C2) estimated density in the (0,inf) interval
zc=lgpdens_cum(bb2-bb1,0,inf);
fprintf('Estimated c statistics for model 1 and 2 respectively:  %.3f, %.3f \n', c1, c2);
fprintf('cumulative probability in the (0,inf) interval:  %.2f \n', zc);

%% IDI
%Estimate R^2 for both models, idi, its density and the cumulative
%probability in the (0,inf) interval, al at time 1

[idi,r1,r2,bbid] = idis(crit1(:,end),crit2(:,end),'rsubstream',1);
zidi=lgpdens_cum(bbid,0,inf);
title('IDI estimated density')
lgpdens(bbid)

fprintf('R^2 statistic for model 1: %.3f \n', r1);
fprintf('R^2 statistic for model 2: %.3f \n', r2);

fprintf('Estimated idi: %.3f ', idi);
fprintf('cumulative probability in the (0,inf) interval: %.2f\n', zidi);

%% EXT AUC

% Ext_AUC for different subsets of tt
Indxtmp{1}=1:1:size(tt,2);
Indx{1}=1:1:size(tt,2);
j=2;
k=round(size(tt,2)/2);
for i=2:k
  Indxtmp{i}=1:i:size(tt,2);
  if length(Indxtmp{i})~=length(Indxtmp{i-1})
    Indx{j}=Indxtmp{i};
    j=j+1;
  end
end

for i=1:size(Indx,2)
  l(i)=length(Indx{i});
end

for i=1:size(Indx,2)
  ea1(i) = ext_auc(crit1(:,Indx{i}),tt(:,Indx{i}),tt(:,Indx{i}(size(Indx{i},2))));
  ea2(i) = ext_auc(crit2(:,Indx{i}),tt(:,Indx{i}),tt(:,Indx{i}(size(Indx{i},2))));
end

extauc1 = ext_auc(crit1,tt,tt(:,end));
extauc2 = ext_auc(crit2,tt,tt(:,end));

fprintf('ExtAUC at end of study for model 1: %.3f \n', extauc1);
fprintf('ExtAUC at end of study for model 2: %.3f \n', extauc2);


% clc, clear, close all
%
% %% compile
% % mex kdtree_build.cpp
% % mex kdtree_k_nearest_neighbors.cpp
% % disp('necessary files compiled.');
%
% %% test1: small test for evaluation and visualizatoin
% rand('seed',1);
% p = rand( 30,  2 ); % input data
% q = [ .5, .5 ]'; k = 5; % query data (buggy)
%
% % execute query by kd-tree and by linear scan
% tree = kdtree_build( p );
% idxs1 = kdtree_k_nearest_neighbors( tree,q,k );
% dist = zeros( size(p,1), 1 );
% for i=1:size(p,1)
%     dist( i ) = sqrt( sum( ( p(i,:)-q' ).^2 ) );
% end
% [IGNORE, idxs2] = sort( dist,'ascend' );
% idxs2 = idxs2(1:k); % keep only first k entries
%
% %%% visualize indexes returned by query
% disp( ['(fullsearch)   kNN indexes: ', sprintf('%d ', idxs2 ) ] );
% disp( ['(kdtreesearch) kNN indexes: ', sprintf('%d ', idxs1 ) ] );
%
% %%% visualize
% xlim( [0 1] );
% ylim( [0 1] );
% hold on; axis equal; axis off;
% plot( p(:,1), p(:,2), '.b');
% plot(q(1), q(2),'.r');
% plot(p(idxs2,1), p(idxs2,2),'or');
% plot(p(idxs1,1), p(idxs1,2),'+y');
% legend('database', 'query', '(matlab) result', '(kdtree) result');
%
% %%% convert input data to C++ format (for testing)
% % for i=1:size(p,1)
% %    disp(sprintf('A[%d][0] = %.2f; A[%d][1] = %.2f;', i-1, p(i,1), i-1, p(i,2) ));
% % end
%
% return

%% test2: extensive random performance and correctness test
% clc, clear, close all;
% NUMTESTS = 10;
% NUMQUERS = 100;
% N = 10000;
% counter = 1;
% kdtree_times = zeros( NUMTESTS*NUMQUERS,1 );
% matlab_times = zeros( NUMTESTS*NUMQUERS,1 );
%
% for j=1:NUMTESTS
%     disp(sprintf('executing test #%d',j));
%     p = rand( 10000,   2 ); % input data
%     q = rand( NUMQUERS,2 ); % query data
%     k = ceil(N/10*rand( 1,1 ));   % query size (number of kNN to extract)
%
%     %% execute query by kd-tree
%     tree = kdtree_build( p );
%
%     for m=1:NUMQUERS
%         % kdtree based
%         tic
%         idxs_kdtree = kdtree_k_nearest_neighbors( tree,q(m,:)',k );
%         kdtree_times( counter ) = toc();
%
%         % matlab based
%         tic
%         dist = zeros( size(p,1), 1 );
%         for i=1:size(p,1)
%             dist( i ) = sqrt( sum( ( p(i,:)-q(m,:) ).^2 ) );
%         end
%         [IGNORE, idxs2] = sort( dist,'ascend' );
%         idxs_matlab = idxs2(1:k);
%         matlab_times( counter ) = toc();
%
%         if( ~all( idxs_kdtree==idxs_matlab) )
%             error('kdtree gave incorrect results');
%         end
%
%         counter = counter + 1;
%     end
% end
%
% subplot( 211 ), plot( kdtree_times ); ylim( [0,0.05] );
% subplot( 212 ), plot( matlab_times ); ylim( [0,0.05] );
% disp( sprintf('kdtree average time (ms): %d ', mean(kdtree_times)/1000 ) );
% disp( sprintf('matlab average time (ms): %d ', mean(matlab_times)/1000 ) );
% return;

% test 3: check if distances returned are correct!
clc, clear, close all
rand('twister',1);
p = rand( 1000,  2 ); % input data
q = [ .5, .5 ];
k = 100;

% execute query by kd-tree and by linear scan
tree = kdtree_build( p );
[idxs1, dists] = kdtree_k_nearest_neighbors( tree,q,k );
dist = zeros( size(p,1), 1 );
for i=1:size(p,1)
    dist( i ) = sqrt( sum( ( p(i,:)-q ).^2 ) );
end
[IGNORE, idxs2] = sort( dist,'ascend' );
idxs2 = idxs2(1:k); % keep only first k entries
disp( sprintf('search is correct?: %d', all(idxs1==idxs2)));

% compare distances against computed
matdist = zeros( length(idxs1),1 );
for i=1:length(idxs1)
    matdist(i) = sqrt( sum((q-p(idxs1(i),:)).^2) );
end
figure(2);
subplot 211, plot( matdist );
subplot 212, plot( dists );


function AC = simpleTRsizing(AC)
% gives us R, sigma, Omega, c, TS, and b for tail rotor

%% check TSoffset value
TSoffset = 7.5; %m/s slower than main rotor
ARgoal = 6;

AC.TRotor.TS = AC.Rotor.TS - TSoffset;

discarea = AC.Rotor.DA;
discloading = AC.W.AUM./discarea;
ratio = 1./(7.15-.0553*discloading);
AC.TRotor.R = AC.Rotor.R.*ratio;

AC.TRotor.DA = pi*AC.TRotor.R.^2;

sigma_mr = AC.Rotor.sigma;
AC.TRotor.sigma = 1.5277 * sigma_mr +.0461; % Trendline from historical aircraft

AC.TRotor.b = round(pi*AC.TRotor.sigma*ARgoal);

AC.TRotor.c = AC.TRotor.sigma*AC.TRotor.DA./(AC.TRotor.b*AC.TRotor.R);

AC.TRotor.Omega = AC.TRotor.TS/AC.TRotor.R;

AC.TRotor.X = AC.Rotor.R+1.1*AC.TRotor.R;

AC.TRotor.BA = AC.TRotor.sigma*AC.TRotor.DA;
end


function AngleAvg = AverageAnglesRadians(InA)
%       Robert Goldstein
%       Schepens Eye Research Institute
%       robert.goldstein@schepens.harvard.edu
    theNumerator = sum(sin(InA));
    theDenominator = sum(cos(InA));
    if(theDenominator ==0)
        AngleAvg = sign(theNumerator)*pi/2;
    else
        AngleAvg = atan2(theNumerator,theDenominator);
    end

end


function [Rtrunc,q,u,v]=nmt_svdtrunc(R,signalspace)
% [Rtrunc,q,u,v] = nmt_svdtrunc(R,signalspace)
%
% Allows user to reject undesired SVD components of an arbitrary matrix R.
% It can be used to, e.g., reduce the matrix's rank.
%
% signalspace should be vector of which components to include, e.g. [1 2]
% for the equivalent of cfg.reducerank=2

[u,q,v]=svd(R,'econ');

notsignalspace = setdiff(1:length(q),signalspace);
q(notsignalspace,notsignalspace)=0;
Rtrunc = u*q*v';


classdef transform
% Implements different versions of wavelet transform
% n=1024, J=10, L=6
% L - J = 10 - 6 = 4.
% s_10 = s_6 + d_6 + d_7 + d_8 + d_9
% j=J-1:-1:L = [9, 8, 7, 6]
% n = 256, J=8, L=0
% s_8 = s_0 + d_0 + d_1 + d_2 + d_3 + d_4 + d_5 + d_6 + d_7.
% s_J = s_L + sum(L <= j < J) d_j.

methods(Static)

    function w = forward_periodized_orthogonal(qmf, x, L)
        % Computes the forward wavelet transform of x
        %
        % Uses the periodized version of x
        % with an orthogonal wavelet basis
        % length of x must be dyadic.
        if nargin < 3
            % We perform full wavelet decomposition
            L = 0;
        end

        % Let's get the dyadic length of x and verify that
        % length of x is a power of 2.
        [n, J, consistent] = spx.wavelet.dyad_length(x);
        if ~consistent
            error('x must be of dyadic length');
        end
        if L >= J
            error('L must be smaller than dyadic index of x');
        end
        % We will work with row vectors in this function.
        col = false;
        if iscolumn(x)
            col = true;
            % Convert it to row vector
            x = x';
        end
        % Create the storage for wavelet coefficients.
        w = zeros(1, n);
        for j=J-1:-1:L
            % Start from the finest level and keep going down.
            % identify the hipass component of x and downsample it.
            c = spx.wavelet.hi_pass_down_sample(qmf, x);
            % Identify the locations where the hipass component will be stored.
            indices = spx.wavelet.dyad(j);
            w(indices) = c;
            % Replace x with its low pass downsampled version
            x = spx.wavelet.lo_pass_down_sample(qmf, x);
        end
        % Store the remaining contents of x in the beginning of array
        w(1:(2^L)) = x;
        if col
            % Convert the wavelet coefficients to a column vector
            w = w';
        end
    end

    function x = inverse_periodized_orthogonal(qmf, w, L)
        % Computes the inverse wavelet transform of x
        %
        % Uses the periodized version of x
        % with an orthogonal wavelet basis
        % length of x must be dyadic.
        if nargin < 3
            % We perform full wavelet composition
            L = 0;
        end

        % Let's get the dyadic length of w and verify that
        % length of w is a power of 2.
        [n, J, consistent] = spx.wavelet.dyad_length(w);
        if ~consistent
            error('w must be of dyadic length');
        end
        if L >= J
            error('L must be smaller than dyadic index of w');
        end
        % We will work with row vectors in this function.
        col = false;
        if iscolumn(w)
            col = true;
            % Convert it to row vector
            w = w';
        end
        % initialize x with its coerce approximation
        x = w(1:2^L);
        for j=L:J-1
            % Identify the locations where the hipass component is stored.
            indices = spx.wavelet.dyad(j);
            c = w(indices);
            % Compute the low pass portion of the next level of approximation
            x_low = spx.wavelet.up_sample_lo_pass(qmf, x);
            if iscolumn(x_low)
                x_low = x_low';
            end
            % Compute the high pass portion of the next level of approximation
            x_hi = spx.wavelet.up_sample_hi_pass(qmf, c);
            if iscolumn(x_hi)
                x_hi = x_hi';
            end
            % Compute the next level approximation of x
            x = x_low + x_hi;
        end
        if col
            % Convert the resulting vector to a column vector
            x = x';
        end
    end

end

end


  function [xs, info] = l1_regress_admm1(yi, A, varargin)
%|function [xs, info] = l1_regress_admm1(yi, A, varargin)
%|
%| l1 regression, minimizing cost function
%| cost(x) = pot( y - A x ) where pot(r) = |r|_1 by default.
%| minimized via ADMM algorithm with v = A x split.
%|
%| in
%|	yi	[M 1]		noisy data
%|	A	[M N]		matrix
%|
%| options
%|	x0	[N 1]		initial estimate (default: A \ yi)
%|	shrink			shrink function for pot: shrink(x, reg)
%|				(default: l1, soft thresholding)
%|
%|	niter			# total iterations (default: 1)
%|					(max # if tol used)
%|	isave	[]		list of iterations to archive (default: 'last')
%|	userfun	@		user defined function handle (see default below)
%|					taking arguments (x, iter, userarg{:})
%|	userarg {}		user arguments to userfun (default {})
%|	stop_diff_tol		stop iterations if norm(xnew-xold)/norm(xnew)
%|				is less than this unitless value.  default: 0
%|	stop_diff_norm		use norm(., type) for stop rule
%|				choices: 1 | 2 (default) | inf
%|	chat	0|1		verbosity (default 0)
%|
%| out
%|	xs	[N niter]	estimates each iteration
%|	info	[niter 1]	time each iteration (for default userfun)
%|
%| Copyright 2013-03-21, Jeff Fessler, University of Michigan

if nargin < 1, ir_usage, end
if nargin == 1 && streq(yi, 'test'), l1_regress_example, return, end

% defaults
arg.x0 = [];
arg.C = [];
arg.beta = 1; % regularization parameter
arg.shrink = [];
arg.rho = 1; % AL penalty parameter

arg.niter = 1;
arg.isave = [];
arg.userfun = @userfun_default;
arg.userarg = {};
arg.stop_diff_tol = 0;
arg.stop_diff_norm = 2;
arg.chat = 0;

arg = vararg_pair(arg, varargin);

if isempty(arg.x0)
	arg.x0 = A \ yi;
end

x = arg.x0;

if isempty(arg.shrink)
	tmp = potential_fun('l1', 1);
	shrink = @(z, reg) tmp.shrink(z, reg);
else
	shrink = arg.shrink;
end

arg.isave = iter_saver(arg.isave, arg.niter);
if arg.stop_diff_tol
	norm_diff = @(x) norm(x(:), arg.stop_diff_norm);
end

cpu etic

np = numel(x);
xs = zeros(np, length(arg.isave));
if any(arg.isave == 0)
	xs(:, arg.isave == 0) = x(:);
end

eta = 0; % dual variable

rho = arg.rho;

% iterate
for iter = 1:arg.niter
	ticker(mfilename, iter, arg.niter)

	xp = x; % previous

	% update primal
	x = A \ (yi + eta);

	% update auxiliary
	tmp = A * x - eta - yi;
	vv = yi + shrink(tmp, 1 / rho);

	% update multiplier
	tmp = A * x - vv;
	eta = eta - tmp;

	if any(arg.isave == iter)
		xs(:, arg.isave == iter) = x;
	end
	info(iter,:) = arg.userfun(x, iter, arg.userarg{:});

	% check norm(xnew-xold) / norm(xnew) vs threshold
	if iter > 1 && arg.stop_diff_tol
		ratio = norm_diff(x - xp) / norm_diff(x);
		if ratio < arg.stop_diff_tol
			if 1 || arg.chat
				printm('stop at iteration %d, diff %g < %g', ...
					iter, ratio, arg.stop_diff_tol)
			end
			if isequal(arg.isave, arg.niter) % saving only last?
				xs = x; % save the 'final' iterate
			else % saving many iterates?
				xs(:, arg.isave > iter) = []; % clear out unused
			end
		break
		end
	end
end

% default user function.
% using this evalin('caller', ...) trick, one can compute anything of interest
function out = userfun_default(x, iter, varargin)
%pr minmax(x)
out = [cpu('etoc')];


function h= rbmVtoH(model, v)
%go from visible to hidden based on type

if isequal(model.type, 'BB')
    h= logistic(v*model.W + repmat(model.b,size(v,1),1));
elseif isequal(model.type, 'BG')
    h= v*model.W + repmat(model.b,size(v,1),1);
elseif isequal(model.type, 'GB')
    h= logistic(v*model.W/model.sigma + repmat(model.b,size(v,1),1));
end


function [FxT_N, FyT_N] = TireModel(lambda_perc, alpha_rad, Fz_N, PacFrontLat, PacRearLat, PacFrontLong, PacRearLong)
  % Pacjeka tire model based on a four coefficient approach
  % evaluates the lateral and longitudinal tire forces using the specified
  % pacjeka models for front and rear

  % initialize
  FxT_N = zeros(4, 1);
  FyT_N = zeros(4, 1);

  FxT_N(1:2) = PacModel(lambda_perc(1:2)./100, PacFrontLong, Fz_N(1:2));
  FxT_N(3:4) = PacModel(lambda_perc(3:4)./100, PacRearLong, Fz_N(3:4));
  FyT_N(1:2) = PacModel(alpha_rad(1:2), PacFrontLat, Fz_N(1:2));
  FyT_N(3:4) = PacModel(alpha_rad(3:4), PacRearLat, Fz_N(3:4));
end

function F = PacModel(x, Pac, Fz_N)
% evaluate the pacejka model for a given slip quantity x and pacejka coefficients Pac
% http://www.edy.es/dev/docs/pacejka-94-parameters-explained-a-comprehensive-guide/

% in addition, Pac(6) provides a factor load degressivity: eps_load <= 0

  F = Fz_N.*(Pac(3)+Pac(6)*((Fz_N-Pac(5))/Pac(5))).*sin(Pac(2).*atan(Pac(1).*x - Pac(4).*(Pac(1).*x - atan(Pac(1).*x))));

end


% demo VB-learning (free learner)
% This demo simulates a VB agent that reacts to outcomes by choosing the
% action that maximizes the expected gain. Here, the agent learns the
% distribution of outcomes associated with each available action. The key
% trick is that this distribution is summmarized in terms of its two first
% moments. The agent a priori belives that these can drift over time, with
% transition variances (i.e. volatilities) exp(theta), where theta is the
% 2x1 evolution parameter vector.
% NB: there are 2 posteriro moments per moment of the outcome distribution,
% which is action-dependent. With 2 available actions, this means there are
% 2x2x2=8 hidden states in this model (to be compared with 2 Q-values for
% RL).
% NB2: the action emission law does not use any utility mapping yet!

close all
clear variables

% simulation parameters
theta = [1;-2]; % volatilities
phi = log(2); % inverse temperature = 4

f_fname = @f_VBfree;
g_fname = @g_ExpUtil;
h_fname = @h_randOutcome;

% allocate feedback struture for simulations
u0 = repmat([ones(1,50),0*ones(1,50)],1,4); % 'correct' answers
fb.inH.u0 = u0;
fb.inH.er = 1; % expected reward when correct answer
fb.inH.vr = .1; % reward variance
fb.h_fname = h_fname;
fb.indy = 1;
fb.indfb = 2;

% choose dummy initial conditions
x0 = repmat([0;1;0;1],2,1);
u = zeros(2,size(fb.inH.u0,2)+1);

n_t = length(u); % number of trials

dim = struct('n',8,'n_theta',2,'n_phi',1);

priors.muPhi = zeros(dim.n_phi,1);
priors.SigmaPhi = 1e0*eye(dim.n_phi);
priors.muTheta = [0;0];
priors.SigmaTheta = 1e0*eye(dim.n_theta);
% priors.SigmaTheta(2,2) = 0;
priors.muX0 = x0;%zeros(dim.n,1);
priors.SigmaX0 = 0e0*eye(dim.n);
priors.a_alpha = Inf;
priors.b_alpha = 0;

options.priors = priors;
options.sources = struct('type',1 ,'out', 1); % one binomial observation;
options.skipf = zeros(1,n_t);
options.skipf(1) = 1; % apply identity mapping from x0 to x1.

[y,x,x0,eta,e,u] = VBA_simulate (n_t,f_fname,g_fname,theta,phi,u,Inf,Inf,options,x0,fb);

figure
plot(y-e,'r')
hold on
plot(y,'kx')
legend({'p(y=1|theta,phi,m)','binomial data samples'})
figure
ti = {'mu1: E[1st moment of u^{o}]','s1: V[1st moment of u^{o}]','mu2: E[log- 2nd moment of u^{o}]','s2: V[log- 2nd moment of u^{o}]'};
for i=1:4
    subplot(2,2,i),plot(x([i,i+4],:)'),title(ti{i})
end
drawnow
VBA_getSubplots ();

[posterior,out] = VBA_NLStateSpaceModel(y,u,f_fname,g_fname,dim,options);

displayResults(posterior,out,y,x,x0,theta,phi,Inf,Inf);


function [sigma,C] = calcBSImpVol(cp,P,S,K,T,r,q)
%
% [sigma,C] = calcBSImpVol(cp,P,S,K,T,r,q)
%
% Calculates Black-Scholes Implied Volatility Surface for an Option Price Matrix.
% Uses Li's Rational Function Approximator for the Initial Estimate, followed by
% 3rd-Order Householder's Root Finder (i.e. using vega,vomma & ultima) for greater
% convergence rate and wider domain-of-convergence relative to Newton-Raphson. Both
% Li's Approximator and the Root Finder are calculated matrix-wise (i.e.
% fully vectorized) for increased efficiency.
%
%
%   Input Parameters
%       cp      Call[+1],Put[-1]  [m x n],[1 x 1]
%       P       Option Price Matrix [m x n]
%       S       Underlying Price [1 x 1]
%       K       Strike Price [m x n]
%       T       Time to Expiry [m x n]
%       r       Continuous Risk-Free Rate [m x n],[1 x 1]
%       q       Continuos Div Yield [m x n],[1 x 1]
%
%   Output Parameters
%       sigma   Implied Volatility  [m x n]
%       C       Convergence Flag  [m x n]
%
%
%
%   Example 1
%     S = 100; K = (40:25:160)'; T = (0.25:0.25:1)'; % Define Key Variables
%     r = 0.01; q = 0.03;
%     cp = 1; % i.e. call
%     P = ...
%         [[59.3526805861312,34.4154741312210,10.3406451776045,0.501199199160055,0.0101623685145268;];
%         [58.7107005379958,33.8563481863964,10.9917759513981,1.36915029885860,0.143324063090580;];
%         [58.0742593358310,33.3567195106962,11.5012247391034,2.12859686975881,0.400045353619436;];
%         [57.4444414750070,32.9126689586500,11.9027988146544,2.77274776123341,0.708059729236718;];];
%     [mK,mT] = meshgrid(K,T);
%     [sigma,C] = calcBSImpVol(cp,P,S,mK,mT,r,q);
%     mesh(mK,mT,sigma);
%
%     Example 2
%       S = 100; K = (40:25:160)'; T = (0.25:0.25:1)'; % Define Key Variables
%       cp = [ones(4,3),-ones(4,2)]; % [Calls[4,3],Puts[4,2]]
%       R = 0.01*repmat([1.15;1.10;1.05;1],1,5); %
%       Q = 0.03*repmat([1.3;1.2;1.1;1],1,5);
%       P = ...
%           [[59.1445725607811,34.2167401269277,10.1798771553458,16.1224863211251,40.5779719086946];
%           [58.4355054500906,33.5945826994415,10.7977275764632,17.4776751735401,41.1533978186314];
%           [57.8694061672804,33.1636044111551,11.3636963648521,18.6294544130139,41.7369813312724];
%           [57.4444414750070,32.9126689586500,11.9027988146694,19.5839252875422,42.2704830992694]];
%       [mK,mT] = meshgrid(K,T);
%       [sigma,C] = calcBSImpVol(cp,P,S,mK,mT,R,Q);
%       mesh(mK,mT,sigma);
%       hold on; scatter3(mK(:),mT(:),sigma(:),60,[0,0,0],'filled'); hold off
%       xlabel('Strike'); ylabel('Expiry'); zlabel('Volatility');
%
% References:
%   1)  Li, 2006, "You Don't Have to Bother Newton for Implied Volatility"
%       http://papers.ssrn.com/sol3/papers.cfm?abstract_id=952727
%   2)  http://en.wikipedia.org/wiki/Householder's_method
%   3)  http://en.wikipedia.org/wiki/Greeks_(finance)
%
%% APPLY LI's RATIONAL-FUNCTION APPROXIMATOR
[g,h] = size(P);
if isscalar(r); r = r*ones(g,h); end
if isscalar(q); q = q*ones(g,h); end
if isscalar(cp); cp = cp*ones(g,h); end
p = [-0.969271876255; 0.097428338274; 1.750081126685];
m = ones(1,1,14);
m(:) = [...
    6.268456292246;
    -6.284840445036;
    30.068281276567;
    -11.780036995036;
    -2.310966989723;
    -11.473184324152;
    -230.101682610568;
    86.127219899668;
    3.730181294225;
    -13.954993561151;
    261.950288864225;
    20.090690444187;
    -50.117067019539;
    13.723711519422];
m = m(ones(g,1),ones(1,h),:); % Repmat to size [g,h]
n = ones(1,1,14);
n(:) = [...
    -0.068098378725;
    0.440639436211;
    -0.263473754689;
    -5.792537721792;
    -5.267481008429;
    4.714393825758;
    3.529944137559;
    -23.636495876611;
    -9.020361771283;
    14.749084301452;
    -32.570660102526;
    76.398155779133;
    41.855161781749;
    -12.150611865704];
n = n(ones(g,1),ones(1,h),:); % Repmat to size [g,h]
i = ones(1,1,14);
i(:) = [0,1,0,1,2,0,1,2,3,0,1,2,3,4];
i = i(ones(g,1),ones(1,h),:); % Repmat to size [g,h]
j = ones(1,1,14);
j(:) = [1,0,2,1,0,3,2,1,0,4,3,2,1,0];
j = j(ones(g,1),ones(1,h),:); % Repmat to size [g,h]
x = log(S.*exp((r-q).*(T))./K); % Calculate Normalized Moneyness Measure
P(cp==-1) = P(cp==-1) + S.*exp(-q(cp==-1).*T(cp==-1)) - K(cp==-1).*exp(-r(cp==-1).*T(cp==-1)); % Convert Put to Call by Parity Relation
c = P./(S.*exp(-q.*(T))); % Normalized Call Price
x = x(:,:,ones(1,14)); % Repmat to 3d size 14
c = c(:,:,ones(1,14)); % Repmat to 3d size 14
% Rational Function -  Eqn(19) of Li 2006
fcnv = @(p,m,n,i,j,x,c)(p(1).*x(:,:,1) + p(2).*sqrt(c(:,:,1)) + p(3).*c(:,:,1) + (sum(n.*((x.^i).*(sqrt(c).^j)),3))./(1 + sum(m.*((x.^i).*(sqrt(c).^j)),3)));
v1 = fcnv(p,m,n,i,j,x,c); % D- Domain (x<=-1)
v2 = fcnv(p,m,n,i,j,-x,exp(x).*c + 1 -exp(x)); % Reflection for D+ Domain (x>1)
v = zeros(g,h); v(x(:,:,1)<=0)=v1(x(:,:,1)<=0); v(x(:,:,1)>0)=v2(x(:,:,1)>0);
% Domain-of-Approximation is x={-0.5,+0.5},v={0,1},x/v={-2,2}
domainFilter = x(:,:,1)>=-0.5 & x(:,:,1)<=0.5 & v > 0 & v <1 & (x(:,:,1)./v)<=2 & (x(:,:,1)./v)>=-2;
sigma = v./sqrt(T); % v = sigma.*(sqrt(T));
sigma(~domainFilter) = 0.8; % use 0.8 arbtrarily as best vol-guess for out-of-domain points
%% HOUSEHOLDER ROOT-FINDER FOR INCREASED CONVERGENCE
d1fcn = @(sig,C)((log(S./K(C)) + (r(C)-q(C)+sig(C).^2*0.5).*(T(C)))./(sig(C).*sqrt(T(C))));
d2fcn = @(sig,C)((log(S./K(C)) + (r(C)-q(C)-sig(C).^2*0.5).*(T(C)))./(sig(C).*sqrt(T(C))));
callfcn = @(sig,C)( +exp(-q(C).*T(C)).*S.*fcnN(d1fcn(sig,C)) - exp(-r(C).*T(C)).*K(C).*fcnN(d2fcn(sig,C)) );
vegafcn = @(sig,C)(S.*exp(-q(C).*(T(C))).*fcnn(d1fcn(sig,C)).*(sqrt(T(C))));
vommafcn = @(sig,C)(S.*exp(-q(C).*(T(C))).*fcnn(d1fcn(sig,C)).*(sqrt(T(C))).*d1fcn(sig,C).*d2fcn(sig,C)./sig(C));
ultimafcn = @(sig,C)(-S.*exp(-q(C).*(T(C))).*fcnn(d1fcn(sig,C)).*(sqrt(T(C))).*(d1fcn(sig,C).*d2fcn(sig,C).*(1-d1fcn(sig,C).*d2fcn(sig,C))+d1fcn(sig,C).^2+d2fcn(sig,C).^2)./(sig(C).^2));
tolMat=1e-12;
k_max = 10; % 10 Householder Iterations
objfcn = @(sig,C)(P(C) - callfcn(sig,C));
C = true(size(P(:))); err = objfcn(sigma,C); % calculate initial error
C = abs(err)>tolMat; % Convergence Matrix
k = 1; % Initialize Count
while any(C) && k<=k_max % Iterate until sooner of Convergence or Count-limit

    % Calculate Derivatives (Greeks)
    vega = vegafcn(sigma,C); %f'(x_n)
    vomma = vommafcn(sigma,C); %f''(x_n)
    ultima = ultimafcn(sigma,C); %f'''(x_n)
%     % Newton Raphson Method x_n+1 = x_n + f(x_n)/f'(x_n)
%     sigma = sigma  + (err(C)./vega) ;
%     % Halley Method x_n+1 = x_n - f(x_n)/( f'(x_n) - f(x_n)*f''(x_n)/2*f'(x_n))
%     sigma = sigma  - err(C)./(-vega-(-err(C).*vomma./(-2.*vega)));
    % Householder Method x_n+1 = x_n - f(x_n)/( f'(x_n) - f(x_n)*f''(x_n)/2*f'(x_n))
    sigma(C) = sigma(C) - (6.*err(C).*vega.^2 + 3.*err(C).^2.*vomma)./(-6.*vega.^3 - 6.*err(C).*vega.*vomma - err(C).^2.*ultima);

    % Update Error
    err(C) = objfcn(sigma,C); %

    % Ascertain Convergence to Tolerance
    C = abs(err)>tolMat; % Convergence Matrix

    % Increment Count
    k = k + 1;
end
sigma(C) = NaN; % any remaining sigma are not worth calculating
end
%% Gaussian Subfunctions
function p=fcnN(x)
p=0.5*(1.+erf(x./sqrt(2)));
end
%
function p=fcnn(x)
p=exp(-0.5*x.^2)./sqrt(2*pi);
end


clear all; close all; clc;
number_of_points = 10;
M = spx.discrete.steiner_system.ss_2(number_of_points);
[m, n] = size(M);
fprintf('Number of points: %d\n', m);
fprintf('Number of blocks: %d\n', n);
fprintf('Blocks:\n');
for i=1:n
    block = M(:, i);
    block = find(block == 1);
    fprintf('%d ', block);
    fprintf('\n');
end


function [sizeROI] = getSize(ROIonly,pixelW,sliceS)
% -------------------------------------------------------------------------
% function [sizeROI] = getSize(ROIonly,pixelW,sliceS)
% -------------------------------------------------------------------------
% DESCRIPTION:
% This function computes the size (longest diameter) of the region of
% interest (ROI) of an input volume.
% -------------------------------------------------------------------------
% INPUTS:
% - ROIonly: 3D array, with voxels outside the ROI set to NaNs.
% - pixelW: Pixel width, or in-plane resolution, in mm.
% - sliceS: Slice spacing, in mm.
% -------------------------------------------------------------------------
% OUTPUTS:
% - sizeROI: Longest diameter of the ROI, in mm.
% -------------------------------------------------------------------------
% AUTHOR(S): Martin Vallieres <mart.vallieres@gmail.com>
% -------------------------------------------------------------------------
% HISTORY:
% - Creation: May 2015
%--------------------------------------------------------------------------
% STATEMENT:
% This file is part of <https://github.com/mvallieres/radiomics/>,
% a package providing MATLAB programming tools for radiomics analysis.
% --> Copyright (C) 2015  Martin Vallieres
%
%    This package 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 package is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this package.  If not, see <http://www.gnu.org/licenses/>.
% -------------------------------------------------------------------------

% IMPORTANT: SMALL ERROR DUE TO Z EXTREMA DEFINED BY CENTER OF VOXELS, AND
% NOT BY EXTREME FACE DISTANCES OF VOXELS (as in regionprops.m). ANOTHER
% WAY WOULD BE TO NOT CONSIDER FACE EXTREMA AND ONLY CENTER OF VOXEL. ONE
% OR THE OTHER SOLUTION SHOULD BE USED, BUT NOT A HYBRID SOLUTION LIKE
% RIGHT NOW.
% --> TO BE CORRECTED ON NEXT RELEASE (message written on Sep 12, 2016)

mask = ~isnan(ROIonly); % Find mask covering the ROI
vectX = zeros(1,8*size(mask,3));
vectY = zeros(1,8*size(mask,3));
vectZ = zeros(1,8*size(mask,3));

for i = 1:size(mask,3)
    temp = regionprops(mask(:,:,i),'Extrema');
    try
        temp = temp.Extrema;
        temp = temp';
        vectX((i-1)*8 + 1:i*8) = temp(1,:) * pixelW;
        vectY((i-1)*8 + 1:i*8) = temp(2,:) * pixelW;
        vectZ((i-1)*8 + 1:i*8) = (i-1) * sliceS;
    catch % Will always work, except when the first slice contains all zeros
        vectX((i-1)*8 + 1:i*8) = vectX(((i-1)-1)*8 + 1:(i-1)*8);
        vectY((i-1)*8 + 1:i*8) = vectY(((i-1)-1)*8 + 1:(i-1)*8);
        vectZ((i-1)*8 + 1:i*8) = vectZ(((i-1)-1)*8 + 1:(i-1)*8);
    end
end

max = 0;
for i = 1:8*size(mask,3) - 1
    for j = i + 1:8*size(mask,3)
       dist = (vectX(i)-vectX(j))^2 + (vectY(i)-vectY(j))^2 + (vectZ(i)-vectZ(j))^2;
       if dist > max
           max = dist;
       end
    end
end

sizeROI = sqrt(max);

end


% RGB normal distributions fit to colors around each pixel

function [meanImage, covarMat] = localRGBnormalDistributions(image, windowRadius, epsilon)

    if ~exist('windowRadius', 'var') || isempty(windowRadius)
        windowRadius = 1;
    end
    if ~exist('epsilon', 'var') || isempty(epsilon)
        epsilon = 1e-8;
    end

    [h, w, ~] = size(image);
    N = h * w;
    windowSize = 2 * windowRadius + 1;

    meanImage = imboxfilt(image, windowSize);
    covarMat = zeros(3, 3, N);

    for r = 1 : 3
        for c = r : 3
            temp = imboxfilt(image(:, :, r).*image(:, :, c), windowSize) - meanImage(:,:,r) .*  meanImage(:,:,c);
            covarMat(r, c, :) = temp(:);
        end
    end

    for i = 1 : 3
        covarMat(i, i, :) = covarMat(i, i, :) + epsilon;
    end

    for r = 2 : 3
        for c = 1 : r - 1
            covarMat(r, c, :) = covarMat(c, r, :);
        end
    end

end


function pass = test_basicArithmetic( pref )
% Check that we can do the basic arithmetic on chebop2 objects.
% Alex Townsend, August 2013.

if ( nargin < 1 )
    pref = chebfunpref();
end
tol = 10*pref.cheb2Prefs.chebfun2eps;

N1 = chebop2(@(x,y,u) diff(u,2,2));
N2 = chebop2(@(x,y,u) x.*diff(u,2,1));
N = N1 + N2;

EXACT = chebop2(@(x,y,u) diff(u,2,2) + x.*diff(u,2,1));
ERROR = N - EXACT;
err = 0; C = ERROR.coeffs;
for jj = 1:size(C,1)
    for kk = 1:size(C,2)
        err = err + norm(C{jj,kk});
    end
end
pass(1) = ( abs(err) < tol );

N1 = chebop2(@(u) diff(u,2,2));
N2 = chebop2(@(x,y,u) x.*diff(u,2,1));
N = N1 + N2;

EXACT = chebop2(@(x,y,u) diff(u,2,2) + x.*diff(u,2,1));
ERROR = N - EXACT;
err = 0; C = ERROR.coeffs;
for jj = 1:size(C,1)
    for kk = 1:size(C,2)
        err = err + norm(C{jj,kk});
    end
end
pass(2) = ( abs(err) < tol );

N1 = chebop2(@(x,y,u) y.*diff(u,2,2));
N2 = chebop2(@(u) diff(u,2,1));
N = N1 + N2;

EXACT = chebop2(@(x,y,u) y.*diff(u,2,2) + diff(u,2,1));
ERROR = N - EXACT;
err = 0; C = ERROR.coeffs;
for jj = 1:size(C,1)
    for kk = 1:size(C,2)
        err = err + norm(C{jj,kk});
    end
end
pass(3) = ( abs(err) < tol );

end


function [hl,ht,hp,ha] = visualize_rotations(rotorder,rotangle_deg,axhandle)

% FUNCTION
%   [hl,ht,hp,ha] = visualize_rotations(rotorder,rotangle_deg,axhandle)
%
% DESCRIPTION
%   Visualize Euler rotation sequences by displaying rotated axes systems {E_r}
%   and rotation planes, where {E_r} is the orthonormal set of vectors [i_r; j_r; k_r]
%
% INPUT ARGUMENTS
%   rotorder = string containing order of rotations, e.g. 'zxy' or 'xyx'
%   rotangle_deg = vector of rotation angles corresponding to rotorder [degrees]
%   axhandle = optional handle to existing axes
%
% OUTPUT ARGUMENTS
%   hl = 3-by-n+1 matrix of line handles for the x,y,z-axes for E0 to En
%   ht = same for axis text labels
%   hp = 3-by-n matrix of handles to the colored patches
%   ha = handle to annotation
%
% EXAMPLE:
%   visualize_rotations('xzxyz',[25 25 -25 45 10])
%
%   or call the function without input arguments to show an example
%
%
% Author: D.J. van Gerwen
% Created: 22-Feb-2011
% Revised: 13-Jun-2013

% Process input arguments
if nargin<2
    rotangle_deg = [25 35 -35]; % [deg]
end
if nargin<1
    rotorder = 'yzx';
end

% Parameters
lw = 1;
fs = 9;
bgc = 'none';
xyztxt = 'xyz';
xlim = [-1 1];
ylim = xlim;
zlim = xlim;
azel = [135,30]; % Azimuth and elevation of view

% Convert angles
rotangle_rad = rotangle_deg/180*pi; % [-]

% Define rotation matrices for {E_i+1}=[R]{E_i} as inline functions (based on right-hand rule)
Rx = inline('[1 0 0; 0 cos(x) sin(x); 0 -sin(x) cos(x)]'); % Rotation about x-axis by angle x
Ry = inline('[cos(y) 0 -sin(y); 0 1 0; sin(y) 0 cos(y)]'); % Rotation about y-axis by angle y
Rz = inline('[cos(z) sin(z) 0; -sin(z) cos(z) 0; 0 0 1]'); % Rotation about z-axis by angle z

% Define global reference system
E = [1 0 0;  % i0
     0 1 0;  % j0
     0 0 1]; % k0 (could use eye(3), but that is less informative)

% Apply transformations: {E1}=[R1]{E0}, {E2}=[R2]{E1}=[R2][R1]{E0}, etc
for r = 1:length(rotorder)
    switch rotorder(r)
        case 'x'
            % Evaluate rotation matrix
            Rr(:,:,r) = Rx(rotangle_rad(r));
            % Define patch
            pv(:,:,r) = [[0 1 0].*Rr(2,:,r); Rr(2,:,r); [0 0 1].*Rr(2,:,r);
                [0 0 1].*Rr(3,:,r); Rr(3,:,r); [0 1 0].*Rr(3,:,r)]; % NOTE: to show triangles, set the third and sixth row to zero
        case 'y'
            % Evaluate rotation matrix
            Rr(:,:,r) = Ry(rotangle_rad(r));
            % Define patch
            pv(:,:,r) = [[1 0 0].*Rr(1,:,r); Rr(1,:,r); [0 0 1].*Rr(1,:,r);
                [0 0 1].*Rr(3,:,r); Rr(3,:,r); [1 0 0].*Rr(3,:,r)]; % NOTE: to show triangles, set the third and sixth row to zero
        case 'z'
            % Evaluate rotation matrix
            Rr(:,:,r) = Rz(rotangle_rad(r));
            % Define patch
            pv(:,:,r) = [[1 0 0].*Rr(1,:,r); Rr(1,:,r); [0 1 0].*Rr(1,:,r);
                [0 1 0].*Rr(2,:,r); Rr(2,:,r); [1 0 0].*Rr(2,:,r)]; % NOTE: to show triangles, set the third and sixth row to zero
    end
    % Transformation to E0
    E(:,:,r+1) = Rr(:,:,r)*E(:,:,r);
end

% Initialize figure
if ~exist('axhandle','var')
    figure('color','white','numbertitle','off','name','Visualize Euler rotation sequence')
    set(gca,'xlim',xlim,'ylim',ylim,'zlim',zlim,'projection','orthographic') % NOTE: projection is either 'orthographic' or 'perspective'
    view(azel)
    axis off
    title(sprintf(['Rotation sequence: %s (%3.0f\\circ' repmat(',%3.0f\\circ',1,length(rotorder)-1) ')'],rotorder,rotangle_deg))
else
    axes(axhandle)
end
%cmp = colormap(lines);
cmp = [1 0 0; 0 0.5 0; 0 0 1; 1 1 0; 0 1 1; 1 0 1];

% Display global reference system {E0} and rotated reference systems {E.}
for r = 1:length(rotorder)+1
    for i = 1:3
        % Axis and label
        hl(i,r) = line([0 E(i,1,r)],[0 E(i,2,r)],[0 E(i,3,r)],'color','k','linewidth',lw);
        ht(i,r) = text('position',E(i,:,r),'string',sprintf('%s_%u',xyztxt(i),r-1),'fontsize',fs,'backgroundcolor',bgc);
        % Create patches
        if r<=length(rotorder)
            pvr = pv(:,:,r);
            for rr = r-1:-1:1
                pvr = pvr*Rr(:,:,rr); % Transform patch vector from local to global reference
            end
            hp(i,r) = patch(pvr(:,1),pvr(:,2),pvr(:,3),cmp(r,:),'edgecolor','none','facealpha',0.1);
        end
    end
end

% Correct axis labels for the rotation axes
for i = 1:3
    for r = strfind(rotorder,xyztxt(i))
        str1 = get(ht(i,r),'string');
        str2 = get(ht(i,r+1),'string');
        set(ht(i,r+1),'string',[str1 ',' str2])
    end
end

% Text
ha = annotation('textbox',[0.4 0 0.6 0.14],'string',{'Colored patches represent components of the local rotation matrix in the plane of rotation. Definitions are according to right-hand rule.'},'edgecolor','none');