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


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


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


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


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 [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


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


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');


function [X_den,P]=denoise_TV_One(Xobs,lambda,l,u,P_init,pars)
%This function implements the FISTA method for TV denoising problems.
%
% INPUT
% Xobs ..............................an observed noisy image.
% lambda ........................ parameter
% pars.................................parameters structure
% pars.MAXITER ..................... maximum number of iterations
%                                                      (Default=100)
% pars.epsilon ..................... tolerance for relative error used in
%                                                       the stopping criteria (Default=1e-4)
% pars.print ..........................  1 if a report on the iterations is
%                                                       given, 0 if the  report is silenced
% pars.tv .................................. type of total variation
%                                                      penatly.  'iso' for isotropic (default)
%                                                      and 'l1' for nonisotropic
%
% OUTPUT
% X_den ........................... The solution of the problem
%                                            min{||X-Xobs||^2+2*lambda*TV(X)}

%Define the Projection onto the box
if((l==-Inf)&&(u==Inf))
    project=@(x)x;
elseif (isfinite(l)&&(u==Inf))
    project=@(x)(((l<x).*x)+(l*(x<=l)));
elseif (isfinite(u)&&(l==-Inf))
     project=@(x)(((x<u).*x)+((x>=u)*u));
elseif ((isfinite(u)&&isfinite(l))&&(l<u))
    project=@(x)(((l<x)&(x<u)).*x)+((x>=u)*u)+(l*(x<=l));
else
    error('lower and upper bound l,u should satisfy l<u');
end

% Assigning parameres according to pars and/or default values
flag=exist('pars', 'var');
if (flag&&isfield(pars,'MAXITER'))
    MAXITER=pars.MAXITER;
else
    MAXITER=100;
end
% if (flag&&isfield(pars,'epsilon'))
%     epsilon=pars.epsilon;
% else
%     epsilon=1e-4;
% end
% if(flag&&isfield(pars,'print'))
%     prnt=pars.print;
% else
%     prnt=1;
% end
if(flag&&isfield(pars,'tv'))
    tv=pars.tv;
else
    tv='iso';
end

[m,n]=size(Xobs);
% clear P; clear R;
if(isempty(P_init))
    P{1}=zeros(m-1,n);    P{2}=zeros(m,n-1);
    R{1}=zeros(m-1,n);    R{2}=zeros(m,n-1);
else
    P{1}=P_init{1};    P{2}=P_init{2};
    R{1}=P_init{1};    R{2}=P_init{2};
end
tk=1;tkp1=1;count=0;i=0;

D=zeros(m,n);%fval=inf;fun_all=[];
while((i<MAXITER)&&(count<5))
%    fold=fval;
    i=i+1;
%     Dold=D;
    Pold=P;
    tk=tkp1;
    D=project(Xobs-lambda*Lforward(R, m, n));
    Q=Ltrans(D, m, n);
    %%%%%%%%%%
    % Taking a step towards minus of the gradient
    P{1}=R{1}+1/(8*lambda)*Q{1};
    P{2}=R{2}+1/(8*lambda)*Q{2};

    %%%%%%%%%%
    % Peforming the projection step
    switch tv
        case 'iso'
            A=[P{1}.^2;zeros(1,n)]+[P{2}.^2,zeros(m,1)];
            A=sqrt(max(A,1));
            P{1}=P{1}./A(1:m-1,:); P{2}=P{2}./A(:,1:n-1);
        case 'l1'
            P{1}=P{1}./(max(abs(P{1}),1));
            P{2}=P{2}./(max(abs(P{2}),1));
        otherwise
            error('unknown type of total variation. should be iso or l1');
    end

    %%%%%%%%%%
    %Updating R and t
    tkp1=(1+sqrt(1+4*tk^2))/2;

    R{1}=P{1}+(tk-1)/(tkp1)*(P{1}-Pold{1});
    R{2}=P{2}+(tk-1)/tkp1*(P{2}-Pold{2});

%     re=norm(D-Dold,'fro')/norm(D,'fro');
%     if (re<epsilon)
%         count=count+1;
%     else
%         count=0;
%     end
    C=Xobs-lambda*Lforward(P, m, n);
    PC=project(C);
%     fval=-norm(C-PC,'fro')^2+norm(C,'fro')^2;
%     fun_all=[fun_all;fval];
end
X_den=D;iter=i;

function X=Lforward(P, m, n)

%       [m2,n2]=size(P{1});
%       [m1,n1]=size(P{2});
%
%       if (n2~=n1+1)
%           error('dimensions are not consistent')
%       end
%       if(m1~=m2+1)
%           error('dimensions are not consistent')
%       end
%
%       m=m2+1;
%       n=n2;

      X=zeros(m,n);
      X(1:m-1,:)=P{1};
      X(:,1:n-1)=X(:,1:n-1)+P{2};
      X(2:m,:)=X(2:m,:)-P{1};
      X(:,2:n)=X(:,2:n)-P{2};
   end

   function P=Ltrans(X, m, n)
%       [m,n]=size(X);
      P{1}=X(1:m-1,:)-X(2:m,:);
      P{2}=X(:,1:n-1)-X(:,2:n);
   end
end


%IKINE Inverse kinematics of HAL objects
%
% Executes the inverse kinematics of a HAL (human arm-like) object.
% It returns the two sets of possible solutions for the shoulder and
% wrist in two of the four possible permutations - the other two may be
% created from the others.
%
% Copyright (C) Bryan Moutrie, 2013-2014
% Licensed under the GNU Lesser General Public License
% see full file for full statement
%
% This file modifies file(s) from The Robotics Toolbox for MATLAB (RTB)
% by Peter Corke (www.petercorke.com), see file for statement
%
% Syntax:
%  (1) [q1, q2] = hal.ikine(Th, phi)
%  (2) [q1, q2] = hal.ikine(Th)
%
%  (2) is as per (1) but uses phi := (1)
%
% Outputs:
%  q1 : First family of solutions (mx7 matrix where m = size(Th,3))
%  q2 : Second family of solutions (mx7 matrix where m = size(Th,3))
%
% Inputs:
%  Th  : Hand frame(s), may be a 4x4xm array of m frames
%  phi : Swivel angle,
%        (0) 0     : Uses phi = 0
%        (1)       : Uses the z-axis of Tw for the z-axis of Ts, via
%                     projection
%        (2) 'h2g' : Is the hand-2-goal method, s.t. z-axis of Ts is
%                     the cross product of the swivel axis and g
%        (3) step  : step is a scalar >0 so that the swivel angle
%                      is added as an an extra dimension, with phi =
%                      median human range +/- step increments up to the
%                      human limits. The swivel angle increases with
%                      the increasing index of the extra dimension.
%        (4) PHI   : PHI uses an explicit value, the number of elements
%                     in PHI must be a multiple of the number of hand
%                     points/frames
%
% See also h2fsu HAL.gikine HAL.wikine

% LICENSE STATEMENT:
%
% This file is part of pHRIWARE.
%
% pHRIWARE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as
% published by the Free Software Foundation, either version 3 of
% the License, or (at your option) any later version.
%
% pHRIWARE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU Lesser General Public
% License along with pHRIWARE.  If not, see <http://www.gnu.org/licenses/>.
%
% RTB LIBRARY:
%
% Copyright (C) 1993-2014, by Peter I. Corke
% http://www.petercorke.com
% Released under the GNU Lesser General Public license,
% Modified 16/6/2014 (HAL is a subclass of SerialLink)

function [q1, q2] = ikine(hal, Th, phi)

if nargin == 2
    [Tf, ~, Tu] = hal.h2fsu(Th);
else
    [Tf, ~, Tu] = hal.h2fsu(Th, phi);
end

[q1g, q2g] = hal.gikine(Tu);

qe = squeeze(pi/2 - acos(dot(Tu(1:3,2,:),Tf(1:3,1,:))));

[q1w, q2w] = wikine(Th, Tf);

q1 = [q1g, qe, q1w];
q2 = [q2g, qe, q2w];

end


%  Figure 4.9      Feedback Control of Dynamic Systems, 6e
%                   Franklin, Powell, Emami
% Figure 4.9   PID control of motor speed
% function [np, dp]= pid(b,J,K,L,R,kp,ki,kd)
% function to compute the equations of a d.c. motor with inductance.
% and compute the response under control
clf;
K=.0670; L1=0.1; J1=0.0113; R=0.45; b=0.0280;
kp=3; ki= 15; kd=0.3;
np=K;
dp=[L1*J1 R*J1+b*L1 R*b+K*K];
dclp=[L1*J1 R*J1+b*L1 R*b+K*K+K*kp];
nclp=K*kp;
nclpw=[L1 R];
dclpi=[L1*J1 R*J1+b*L1 R*b+K*K+K*kp K*ki];
nclpi=[K*kp K*ki];
nclpiw=[L1 R 0];
dclpid=[  L1*J1 R*J1+b*L1+K*kd R*b+K*K+K*kp K*ki];
nclpid=[K*kd K*kp K*ki];
nclpidw=[L1 R 0];
sysp=tf(nclp,dclp);
syspw=tf(nclpw,dclp);
syspi=tf(nclpi,dclpi);
syspiw=tf(nclpiw,dclpi);
syspid=tf(nclpid,dclpid);
syspidw=tf(nclpidw,dclpid);
figure(1)
t=0:.01:6;
[y1w,t]=step(syspw,t);
[y2w,t]=step(syspiw,t);
[y3w,t]=step(syspidw,t);
plot(t,y1w,t,y2w,t,y3w);
xlabel('Time (msec)');
ylabel('Amplitude');
title('Fig. 4.9(a) Response of P,PI, and PID control to a disturbance step')
gtext('P')
gtext('PI')
gtext('PID')
nicegrid;

figure(2)
[y1,t]=step(sysp,t);
[y2,t]=step(syspi,t);
[y3,t]=step(syspid,t);
plot(t,y1,t,y2,t,y3);
xlabel('Time (msec)');
ylabel('Amplitude');
title('Fig. 4.9 (b) Response of P,PI, and PID control to a reference step')
gtext('P')
gtext('PI')
gtext('PID')
nicegrid;


function [R]=hist2res(H,fun)
% Evaluates Histogram data
% [R]=hist2res(H)
%
% [y]=hist2res(H,fun)
%	estimates fun-statistic
%
% fun	'mean'	mean
%	'std'	standard deviation
%	'var'	variance
%	'sem'	standard error of the mean
%	'rms'	root mean square
%	'meansq' mean of squares
%	'sum'	sum
%	'sumsq'	sum of squares
%	'CM#'	central moment of order #
%	'skewness' skewness
%	'kurtosis' excess coefficient (Fisher kurtosis)
%
% see also: NaN/statistic
%
% REFERENCES:
% [1] C.L. Nikias and A.P. Petropulu "Higher-Order Spectra Analysis" Prentice Hall, 1993.
% [2] C.E. Shannon and W. Weaver "The mathematical theory of communication" University of Illinois Press, Urbana 1949 (reprint 1963).
% [3] http://www.itl.nist.gov/
% [4] http://mathworld.wolfram.com/

% This program is free software; you can redistribute it and/or
% modify it under the terms of the GNU General Public License
% as published by the Free Software Foundation; either version 2
% of the License, or (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

%	$Id: hist2res.m 2202 2009-10-27 12:06:45Z schloegl $
%	Copyright (c) 1996-2002,2006 by Alois Schloegl <a.schloegl@ieee.org>
%    	This is part of the BIOSIG-toolbox http://biosig.sf.net/

if strcmp(H.datatype,'HISTOGRAM')

elseif strcmp(H.datatype,'qc:histo')
	HDR = H;
	if isfield(H,'THRESHOLD'),
		TH  = H.THRESHOLD;
	else
		TH = repmat([-inf,inf],HDR.NS,1);
	end;
	HIS = H.HIS;

	% remove overflowing samples
	HIS.N = sumskipnan(HIS.H);
	for k = 1:size(HIS.H,2);
		t = HIS.X(:,min(k,size(HIS.X,2)));
		HIS.H(xor(t<=min(TH(k,:)), t>=max(TH(k,:))),k) = 0;
	end;
	Nnew = sumskipnan(HIS.H);
	R.ratio_lost = 1-Nnew./HIS.N;
	HIS.N = Nnew;

	% scale into physical values
	if H.FLAG.UCAL,
		%t = HIS.X;
		%for k=1:length(HDR.InChanSelect),
		%	HIS.X(:,k) = t(:,min(size(t,2),k))*HDR.Calib(k+1,k)+HDR.Calib(1,k);
		%end;
		HIS.X = [ones(size(HIS.X,1),1),repmat(HIS.X,1,size(HIS.H,2)./size(HIS.X,2))]*H.Calib;
	end;
	H = HIS;
else
        fprintf(2,'ERROR: arg1 is not a histogram\n');
        return;
end;
if nargin<2, fun=[]; end;

global FLAG_implicit_unbiased_estimation;
%%% check whether FLAG was already defined
if exist('FLAG_implicit_unbiased_estimation')~=1,
	FLAG_implicit_unbiased_estimation=[];
end;
%%% set DEFAULT value of FLAG
if isempty(FLAG_implicit_unbiased_estimation),
	FLAG_implicit_unbiased_estimation=logical(1);
end;

sz 	= size(H.H)./size(H.X);
R.N 	= sumskipnan(H.H,1);
R.SUM 	= sumskipnan(H.H.*repmat(H.X,sz),1);
R.SSQ 	= sumskipnan(H.H.*repmat(H.X.*H.X,sz),1);
%R.S3P 	= sumskipnan(H.H.*repmat(H.X.^3,sz),1);	% sum of 3rd power
R.S4P 	= sumskipnan(H.H.*repmat(H.X.^4,sz),1);	% sum of 4th power
%R.S5P 	= sumskipnan(H.H.*repmat(H.X.^5,sz),1);	% sum of 5th power

R.MEAN	= R.SUM./R.N;
R.MSQ   = R.SSQ./R.N;
R.RMS	= sqrt(R.MSQ);
R.SSQ0  = R.SSQ-R.SUM.*R.MEAN;		% sum square of mean removed

if FLAG_implicit_unbiased_estimation,
    n1 	= max(R.N-1,0);			% in case of n=0 and n=1, the (biased) variance, STD and STE are INF
else
    n1	= R.N;
end;

R.VAR  	= R.SSQ0./n1;	     		% variance (unbiased)
R.STD  	= sqrt(R.VAR);		     	% standard deviation
R.SEM  	= sqrt(R.SSQ0./(R.N.*n1)); 	% standard error of the mean
R.SEV	= sqrt(n1.*(n1.*R.S4P./R.N+(R.N.^2-2*R.N+3).*(R.SSQ./R.N).^2)./(R.N.^3)); % standard error of the variance
R.Coefficient_of_variation = R.STD./R.MEAN;

R.CM2	= R.SSQ0./n1;
x       = repmat(H.X,sz) - repmat(R.MEAN,size(H.X,1),1);
R.CM3 	= sumskipnan(H.H.*(x.^3),1)./n1;
R.CM4 	= sumskipnan(H.H.*(x.^4),1)./n1;
%R.CM5 	= sumskipnan(H.H.*(x.^5),1)./n1;

R.SKEWNESS = R.CM3./(R.STD.^3);
R.KURTOSIS = R.CM4./(R.VAR.^2)-3;
R.MAD = sumskipnan(H.H.*abs(x),1)./R.N; % mean absolute deviation

H.PDF = H.H./H.N(ones(size(H.H,1),1),:);
status=warning('off');
R.ENTROPY = -sumskipnan(H.PDF.*log2(H.PDF),1);
warning(status);
R.QUANT = repmat(min(diff(H.X,[],1)),1,size(H.H,2)/size(H.X,2));
R.MAX = max(H.X);
R.MIN = min(H.X);
R.RANGE = R.MAX-R.MIN;

if ~isempty(fun),
        fun=upper(fun);
        if strncmp(fun,'CM',2)
                oo = str2double(fun(3:length(fun)));
                R = sumskipnan(H.PDF.*(x.^oo),1);
    	else
		R = getfield(R,fun);
	end;
end;


function Q=QTaylor(deltaT,xCur,curT,D,dadx,method)
%%QTAYLOR Get the process noise covariance under a nonlinear continuous-
%         time random process specified by the Langevin equation forward in
%         time by a step-size of deltaT using a Taylor scheme with additive
%         noise.
%
%INPUTS: deltaT The size of the single step over which to generate the
%               process noise covariance matrix.
%          xCur The initial target state at time curT.
%          curT The time of the initial state xCur.
%             D The diffusion function in the continuous-time stochastic
%               dynamic model. It takes the state and a time variable as
%               its arguments, D(x,t). Since the process noise must be
%               additive, D(x,t) should not depend on x.
%          dadx A xDimXxDim matrix of the derivative of the drift function
%               with respect to the state at state xCur and time curT.
%               This can also be a function that takes the state and a time
%               variable as its arguments, dadx(x,t).
%        method Set to 0 for the shorter Euler-Maruyama expansion, 1 for
%               the order 1.5 strong Taylor (default), and 2 for the order
%               2.0 weak Taylor.
%
%OUTPUTS: Q The process noise covariance matrix under a nonlinear
%           continuous-time random process specified by the Langevin
%           equation forward in time by a step-size of deltaT using a
%           strong Taylor scheme with additive noise.
%
%The process noise covariance matrix is derived from the stochastic order
%1.5 Taylor scheme with additive noise described in 10.4 of [1].
%
%REFERENCES:
%[1] P. E. Kloeden and E. Platen, Numerical Solution of Stochastic
%    Differential Equations. Berlin: Springer, 1999.
%
%April 2015 David Karnick, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(isa(dadx,'function_handle'))
    dadx=dadx(xCur,curT);
end
if(nargin<6||isempty(method))
    method=1;
end

DCur=D(xCur,curT);

if(method==0)
    Q=deltaT*(DCur*DCur.');
else
    term1=deltaT*(DCur*DCur.');
    term2=(deltaT^2/2)*(DCur*(dadx*DCur).'+(DCur*(dadx*DCur).').');
    if(method==1)
        term3=(deltaT^3/3)*(dadx*DCur)*((dadx*DCur).');
    elseif(method==2)
        term3=(deltaT^3/4)*(dadx*DCur)*((dadx*DCur).');
    end

    Q=term1+term2+term3;
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [xUpdate,MUpdate,DUpdate,innov,Pzz,W]=reducedStateUpdate(xPred,PPred,MPred,DPred,z,R,H)
%%REDSTATEFILTERUPDATE Perform the measurement update step in the reduced
%               state estimator. This is a filter that takes measurements
%               in Cartesian coordinates and assumes that the dynamic model
%               includes an unknown parameter that is somehow bounded, but
%               whose contribution is modeled with a mean and covariance
%               matrix. The filter separates contributions due to
%               measurement errors and dynamic model mismatch errors.
%
%INPUTS: xPred The xDimX1 predicted state estimate.
%        PPred The xDimXxDim predicted total state covariance estimate.
%              This is provided by the state prediction step.
%        MPred The xDimXxDim matrix contributing to the total predicted
%              state covariance matrix based solely on measurement errors.
%        DPred The xDimXzDim matrix of bias coefficients that are supposed
%              to relate target state errors to dynamic model parameter
%              uncertainty.
%            z The zDimX1 measurement vector.
%            R The zDimXzDim measurement covariance matrix.
%            H The optional zDimXxDim measurement matrix. The measurement
%              is modeled as z=H*x+noise. If this parameter is omitted or
%              an empty matrix is passed, then H will be taken as a
%              zDimXxDim identity matrix followed by columns of zeros
%              (Assuming that zDim<=xDim. Otherwise, H must be provided).
%
%OUTPUTS: xUpdate The xDimX1 updated state estimate.
%         MUpdate The xDimXxDim updated state covariance matrix
%                 contribution due to measurement errors.
%         DUpdate The xDimXzDim matrix of updated bias coefficients
%                 contributing to the state covariance matrix.
%      innov, Pzz The zDimX1 innovation and a zDimXzDim matrix Pzz that is
%                 akin to an innovation covariance matrix are returned in
%                 case one wishes to analyze the consistency of the
%                 estimator or use those values in gating or likelihood
%                 evaluation.
%               W The gain used in the update. This can be useful when
%                 gating and using the function calcMissedGateCov.
%
%The filter is taken from [1]. Other applications are described in [1].
%Note that the formulation of the dynamic model requires that the
%dimensionality of the measurements does not vary.
%
%In [1] and [2], no clear method of initializing this type of tracking
%filter is provided. A simple way to initialize the filter would be to use
%two Cartesian converted measurements to obtain a state estimate and
%covariance as one would do with a normal Kalman filter (one could, for
%example, use the KalmanFIRSmoother function) and then set MPrev to the
%covariance value obtained while setting DPrev to zero.
%
%REFERENCES:
%[1] P. Mookerjee and F. Reifler, "Reduced state estimator for systems with
%    parametric inputs," IEEE Transactions on Aerospace and Electronic
%    Systems, vol. 40, no. 2, pp. 446-461, Apr. 2004.
%[2] P. Mookerjee and F. Reifler, "Reduced state estimators for consistent
%    tracking of maneuvering targets," IEEE Transactions on Aerospace and
%    Electronic Systems, vol. 41, no. 2, pp. 608-619, Apr. 2005.
%
%July 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

xDim=size(xPred,1);

if(nargin<7||isempty(H))
	zDim=size(z,1);
    H=[eye(zDim,zDim),zeros(zDim,xDim-zDim)];
end

%Equation 20 in [1].
Pzz=H*PPred*H'+R;

%Ensure symmetry
Pzz=(Pzz+Pzz')/2;

W=PPred*H'/Pzz;
L=eye(xDim,xDim)-W*H;

%Equation 23 in [1].
MUpdate=L*MPred*L'+W*R*W';

%Ensure symmetry
MUpdate=(MUpdate+MUpdate')/2;

%Equation 24 in [1].
DUpdate=L*DPred;

innov=z-H*xPred;
%Equation 26 in [1].
xUpdate=xPred+W*innov;

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function plotsid(t,iflaws,k);
%PLOTSID Schematic interference diagram of FM signals.
%	PLOTSID(T,IFLAWS,K) plots the schematic interference diagram of
%	(analytic) FM signals.
%
%	T : time instants,
%	IFLAWS : matrix of instantaneous frequencies,
%	         with as may columns as signal components.
%	K : distribution		(default : 2):
%	  K = 2     : Wigner-Ville
%	  K = 1/2   : D-Flandrin
%	  K = 0     : Bertrand (unitary)
%	  K = -1    : Unterberger (active)
%	  K = inf   : Margenhau-Hill-Rihaczek
%
%	Example :
%	 Nt=90; [y,iflaw]=fmlin(Nt,0.05,0.25);
%	 [y2,iflaw2]=fmconst(50,0.4);
%	 iflaw(:,2)=[NaN*ones(10,1);iflaw2;NaN*ones(Nt-60,1)];
%	 plotsid(1:Nt,iflaw,0);
%
%	See also PLOTIFL, MIDPOINT.

%	P. Flandrin, September 1995.
%	Copyright (c) 1996 by CNRS (France).
%
%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

if (nargin==1),
 error('at least 2 parameters : t and iflaws');
elseif (nargin==2),
 k=2; % Wigner-Ville
end;

indices=find(1-isnan(iflaws));
if (min(iflaws(indices))<0)|(max(iflaws(indices))>0.5),
 error ('each element of IFLAWS must be between 0 and 0.5');
end;

[iflawrow,iflawcol]=size(iflaws);
tcol=length(t);
clf; figure(gcf); plotifl(t,iflaws);
hold on;
col=['b','m','c','r'];

% auto-terms
for j=1:iflawcol,
 indices=find(1-isnan(iflaws(:,j)));
 Nbpoints=length(indices);
 for i=1:Nbpoints-1,
  ta=       t(indices(i))  *ones(1,Nbpoints-i);
  fa=  iflaws(indices(i),j)*ones(1,Nbpoints-i);
  tb= t(indices(i+1:Nbpoints));
  fb=iflaws(indices(i+1:Nbpoints),j)';
  [ti,fi]=midscomp(ta,fa,tb,fb,k);
  plot(ti,fi,['.',num2str(col(rem(j-1,4)+1))]);
 end;
end;

% cross-terms
for j1=1:iflawcol,
 indices1=find(1-isnan(iflaws(:,j1)));
 Nbpoints1=length(indices1);
 for j2=j1+1:iflawcol,
  indices2=find(1-isnan(iflaws(:,j2)));
  Nbpoints2=length(indices2);
  for i=1:Nbpoints1,
   ta=       t(indices1(i))   *ones(1,Nbpoints2);
   fa=  iflaws(indices1(i),j1)*ones(1,Nbpoints2);
   tb= t(indices2);
   fb=iflaws(indices2,j2)';
   [ti,fi]=midscomp(ta,fa,tb,fb,k);
   plot(ti,fi,'.g')
  end;
 end;
end;

hold off
axis([t(1) t(tcol) 0 0.5]);
grid
if k==2,
 dist=' of the Wigner-Ville distribution';
elseif k==1/2,
 dist=' of the D-Flandrin distribution';
elseif k==0,
 dist=' of the (unitary) Bertrand distribution';
elseif k==-1,
 dist=' of the (active) Unterberger distribution';
elseif k>1/sqrt(eps),
 dist=' of the Margenhau-Hill-Rihaczek distribution';
else
 dist='';
end

title(['Interference diagram',dist,' (k = ',num2str(k),')']);