function [ X ] = hyperFcls( M, U )
%HYPERFCLS Performs fully constrained least squares on pixels of M.
% hyperFcls performs fully constrained least squares of each pixel in M
% using the endmember signatures of U.  Fully constrained least squares
% is least squares with the abundance sum-to-one constraint (ASC) and the
% abundance nonnegative constraint (ANC).
%
% Usage
%   [ X ] = hyperFcls( M, U )
% Inputs
%   M - HSI data matrix (p x N)
%   U - Matrix of endmembers (p x q)
% Outputs
%   X - Abundance maps (q x N)
%
% References
%   "Fully Constrained Least-Squares Based Linear Unmixing." Daniel Heinz,
% Chein-I Chang, and Mark L.G. Althouse. IEEE. 1999.

if (ndims(U) ~= 2)
    error('M must be a p x q matrix.');
end

[p1, N] = size(M);
[p2, q] = size(U);
if (p1 ~= p2)
    error('M and U must have the same number of spectral bands.');
end

p = p1;
X = zeros(q, N);
Mbckp = U;
for n1 = 1:N
    count = q;
    done = 0;
    ref = 1:q;
    r = M(:, n1);
    U = Mbckp;
    while not(done)
        als_hat = inv(U.'*U)*U.'*r;
        s = inv(U.'*U)*ones(count, 1);

        % IEEE Magazine method (http://www.planetary.brown.edu/pdfs/3096.pdf)
        % Contains correction to sign.  Error in original paper.
        afcls_hat = als_hat - inv(U.'*U)*ones(count, 1)*inv(ones(1, count)*inv(U.'*U)*ones(count, 1))*(ones(1, count)*als_hat-1);

        % See if all components are positive.  If so, then stop.
        if (sum(afcls_hat>0) == count)
            alpha = zeros(q, 1);
            alpha(ref) = afcls_hat;
            break;
        end
        % Multiply negative elements by their counterpart in the s vector.
        % Find largest abs(a_ij, s_ij) and remove entry from alpha.
        idx = find(afcls_hat<0);
        afcls_hat(idx) = afcls_hat(idx) ./ s(idx);
        [val, maxIdx] = max(abs(afcls_hat(idx)));
        maxIdx = idx(maxIdx);
        alpha(maxIdx) = 0;
        keep = setdiff(1:size(U, 2), maxIdx);
        U = U(:, keep);
        count = count - 1;
        ref = ref(keep);
    end
    X(:, n1) = alpha;
end

return;


function cycle_floyd_test02 ( )

%*****************************************************************************80
%
%% CYCLE_FLOYD_TEST02 tests CYCLE_FLOYD for F2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 June 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CYCLE_FLOYD_TEST02\n' );
  fprintf ( 1, '  Test CYCLE_FLOYD for F2().\n' );
  fprintf ( 1, '  f2(i) = mod ( 22 * i + 1, 72 ).\n' );

  x0 = 0;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Starting argument X0 = %d\n', x0 );

  [ lam, mu ] = cycle_floyd ( @f2, x0 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Reported cycle length is %d\n', lam );
  fprintf ( 1, '  Expected value is 9\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Reported distance to first cycle element is %d\n', mu );
  fprintf ( 1, '  Expected value is 3\n' );

  return
end


function problem1 ( )

%*****************************************************************************80
%
%% PROBLEM1 is the main program for problem 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 May 2009
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PROBLEM1\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  A test problem for FD1D_HEAT_STEADY.\n' );

  n = 11;

  a = 0.0;
  b = 1.0;

  ua = 0.0;
  ub = 1.0;

  [ x, u ] = fd1d_heat_steady ( n, a, b, ua, ub, @k1, @f1 );

  plot ( x, u )
  title ( 'Problem 1: Uniform K(X)' );
  xlabel ( 'A <= X <= B' );
  ylabel ( 'Temperature' );

  filename = 'problem1_nodes.txt';
  r8mat_write ( filename, 1, n, x );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  X data written to "%s".\n', filename );
  filename = 'problem1_values.txt';
  r8mat_write ( filename, 1, n, u' );
  fprintf ( 1, '  U data written to "%s".\n', filename );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PROBLEM1\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );

  timestamp ( );

  return
end
function value = k1 ( x )

%*****************************************************************************80
%
%% K1 evaluates the heat transfer coefficient K(X).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 May 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the position.
%
%    Output, real VALUE, the value of K(X).
%
  value = 1.0;

  return
end
function value = f1 ( x )

%*****************************************************************************80
%
%% F1 evaluates the right hand side function F(X).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 May 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the position.
%
%    Output, real VALUE, the value of F(X).
%
  value = 0.0;

  return
end


function quadrule_test04 ( )

%*****************************************************************************80
%
%% TEST04 tests GAUSS_LEGENDRE_RULE_COMPUTE and GAUSS_LEGENDRE_RULE_SET.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 March 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST04\n' );
  fprintf ( 1, '  GAUSS_LEGENDRE_RULE_COMPUTE computes\n' );
  fprintf ( 1, '  a Clenshaw-Curtis quadrature rule.\n' );
  fprintf ( 1, '  GAUSS_LEGENDRE_RULE_SET sets\n' );
  fprintf ( 1, '  a Clenshaw-Curtis quadrature rule.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Compare:\n' );
  fprintf ( 1, '    (X1,W1) from GAUSS_LEGENDRE_RULE_SET\n' );
  fprintf ( 1, '    (X2,W2) from GAUSS_LEGENDRE_RULE_COMPUTE\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '     Order        W1            W2            X1            X2\n' );
  fprintf ( 1, '\n' );

  for order = 1 : 3 : 10

    [ x1, w1 ] = gauss_legendre_rule_set ( order );
    [ x2, w2 ] = gauss_legendre_rule_compute ( order );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %8d\n', order );

    for i = 1 : order
      fprintf ( 1, '            %12f  %12f  %12f  %12f\n', ...
        w1(i), w2(i), x1(i), x2(i) );
    end

  end

  return
end


function [K, runtime, Phi] = spkernel(Graphs, features)
% Compute shortest path kernel for a set of node-labeled graphs
% Copyright 2012 Nino Shervashidze
% Input: Graphs - a 1xN array of graphs
% Output: K - NxN kernel matrix K
%         runtime - scalar
%         features - a boolean: 1 if we want to output the feature
%                    vector representation for each graph, 0 otherwise

N=size(Graphs,2);
Ds = cell(1,N); % shortest path distance matrices for each graph
if nargin<3 features=0; end

t=cputime; % for measuring runtime

%%% PREPROCESSING (mainly in order to find out the size of the node
%%% label alphabet, L, and rename labels as 1 ,..., L)
label_lookup=containers.Map();
label_counter=1;
for i=1:N
  for j=1:length(Graphs(i).nl.values)
    str_label=num2str(Graphs(i).nl.values(j));
    % str_label is the node label of the current node of the
    % current graph converted into a string
    if ~isKey(label_lookup, str_label)
      label_lookup(str_label)=label_counter;
      Graphs(i).nl.values(j)=label_counter;
      label_counter=label_counter+1;
    else
      Graphs(i).nl.values(j)=label_lookup(str_label);
    end
  end
end
L=label_counter-1; % L is the size of the node label alphabet
disp(['the preprocessing step took ', num2str(cputime-t), ' sec']);
t=cputime;

% compute Ds and the length of the maximal shortest path over the dataset
maxpath=0;
for i=1:N
  Ds{i}=floydwarshall(Graphs(i).am);
  aux=max(Ds{i}(~isinf(Ds{i})));
  if aux > maxpath
    maxpath=aux;
  end
 % if rem(i,100)==0 disp(i); end
end

sp=sparse((maxpath+1)*L*(L+1)/2,N);
for i=1:N
  labels_aux=repmat(Graphs(i).nl.values,1,length(Graphs(i).nl.values));
  a=min(labels_aux, labels_aux');
  b=max(labels_aux, labels_aux');
  I=triu(~(isinf(Ds{i})));
  Ind=Ds{i}(I)*L*(L+1)/2+(a(I)-1).*(2*L+2-a(I))/2+b(I)-a(I)+1;
  aux=accumarray(Ind,ones(nnz(I),1));
  sp(Ind,i)=aux(Ind);
end
sp=sp(sum(sp,2)~=0,:);
K=full(sp'*sp);
if features Phi=sp; end
runtime=cputime-t;
disp(['kernel computation took ', num2str(cputime-t), ' sec']);
end


%% Generate Test Problems + Results for opti_Install_Test
% Assumes you are in base opti directory!
% All simple, quick problems

%% Linear Programming
clc
clear
no = 5;
lp_tprob = cell(no,1);
lp_sval = zeros(no,1);
i = 1;

%LP 1
f = -[-1, 2]';
A = [2, 1;-4, 4];
b = [5, 5]';
lp_tprob{i} = optiprob('f',f,'ineq',A,b);
lp_sval(i) = -3.75;
i = i + 1;

%LP 2
f = -[50, 100];
A = [10, 5;4, 10; 1, 1.5];
b = [2500, 2000, 450]';
lp_tprob{i} = optiprob('f',f,'ineq',A,b);
lp_sval(i) = -21875;
i = i + 1;

%LP 3
f = [2, 3, 7, 7];
A = [1, 1, -2, -5;-1, 2, 1, 4];
b = [2, -3]';
e = [1, 1];
lb = zeros(4,1);
ub = [30 100 20 1]';
lp_tprob{i} = optiprob('f',f,'mix',A,b,e,'bounds',lb,ub);
lp_sval(i) = 4;
i = i + 1;

%LP 4
f = [1, 2, 3, 7, 8, 8];
A = [5, -3, 2, -3, -1, 2; -1, 0, 2, 1, 3, -3;1, 2, -1, 0, 5, -1];
b = [-5, -1, 3]';
e = [1, 1, 1];
lb = zeros(6,1);
ub = 10*ones(6,1);
lp_tprob{i} = optiprob('f',f,'mix',A,b,e,'bounds',lb,ub);
lp_sval(i) = 3;
i = i + 1;

%LP 5
f = -[1 2 3]';
A = [-1,1,1; 1,-3,1];
b = [20,30]';
Aeq = [1 1 1];
beq = 40;
lb = [0 0 0]';
ub = [40 inf inf]';
lp_tprob{i} = optiprob('grad',f,'ineq',A,b,'eq',Aeq,beq,'bounds',lb,ub);
lp_sval(i) = -97.5;
i = i + 1;

%LP Clean Up + Save
clear f A b e Aeq beq lb ub i no
save 'Utilities/Install/Test Results/lp_test_results.mat'

%% Mixed Integer Linear Programming
clc
clear
no = 5;
milp_tprob = cell(no,1);
milp_sval = zeros(no,1);
i = 1;

%MILP 1
f = -[-1, 2]';
A = [2, 1;-4, 4];
b = [5, 5]';
xint = 'II';
milp_tprob{i} = optiprob('f',f,'ineq',A,b,'int',xint);
milp_sval(i) = -3;
i = i + 1;

%MILP 2
f = [2, 3, 7, 7];
A = [1, 1, -2, -5;-1, 2, 1, 4];
b = [2, 3]';
e = [1, 1];
lb = zeros(4,1);
ub = [30 100 20 1]';
xint = 'CICI';
milp_tprob{i} = optiprob('f',f,'mix',A,b,e,'bounds',lb,ub,'int',xint);
milp_sval(i) = 6;
i = i + 1;

%MILP 3
f = -[592, 381, 273, 55, 48, 37, 23];
A = [3534, 2356, 1767, 589, 528, 451, 304];
b = 119567;
lb = zeros(7,1);
ub = [100 50 33 20 77 44 20]';
xint = 'IIIIIII';
milp_tprob{i} = optiprob('f',f,'ineq',A,b,'bounds',lb,ub,'int',xint);
milp_sval(i) = -19979;
i = i + 1;

%MILP 4
f = -[6 5]';
A = [-3,5; 6,4; 3, -5; -6, -4];
b = [6;9;1;3];
xint = 'BC';
milp_tprob{i} = optiprob('grad',f,'ineq',A,b,'int',xint);
milp_sval(i) = -9.75;
i = i + 1;

%MILP 5
f = -[1 2 3 1]';
A = [-1 1 1 10; 1 -3 1 0];
b = [20;30];
Aeq = [0 1 0 -3.5];
beq = 0;
lb = [0 0 0 2]';
ub = [40 inf inf 3]';
xint = 'CCCI';
%Build & Options
milp_tprob{i} = optiprob('grad',f,'ineq',A,b,'eq',Aeq,beq,'bounds',lb,ub,'int',xint);
milp_sval(i) = -122.5;

clear f A b e Aeq beq lb ub xint i no
save 'Utilities/Install/Test Results/milp_test_results.mat'

%% Quadratic Programming
clc
clear
no = 3;
qp_tprob = cell(no,1);
qp_sval = zeros(no,1);
i = 1;

%QP 1
H = eye(3);
f = -[2 3 1]';
A = [1 1 1;3 -2 -3; 1 -3 2];
b = [1;1;1];
qp_tprob{i} = optiprob('H',H,'f',f,'ineq',A,b);
qp_sval(i) = -2.83333333333333;
i = i + 1;

%QP 2
H = [1 -1; -1 2];
f = -[2 6]';
A = [1 1; -1 2; 2 1];
b = [2; 2; 3];
lb = [0;0];
qp_tprob{i} = optiprob('qp',H,f,'ineq',A,b,'lb',lb);
qp_sval(i) = -8.22222220552525;
i = i + 1;

%QP 3
H = [1 -1; -1 2];
f = -[2 6]';
A = [1 1; -1 2; 2 1];
b = [2; 2; 3];
Aeq = [1 1.5];
beq = 2;
lb = [0;0];
ub = [10;10];
qp_tprob{i} = optiprob('qp',H,f,'eq',Aeq,beq,'ineq',A,b,'bounds',lb,ub);
qp_sval(i) = -6.41379310344827;
i = i + 1;

clear H f A b e Aeq beq lb ub i no
save 'Utilities/Install/Test Results/qp_test_results.mat'

%% MI Quadratic Programming
clc
clear
no = 2;
miqp_tprob = cell(no,1);
miqp_sval = zeros(no,1);
i = 1;

%MIQP 1
H = [1 -1; -1 2];
f = -[2 6]';
A = [1 1; -1 2];
b = [3; 3.5];
miqp_tprob{i} = optiprob('hess',H,'grad',f,'ineq',A,b,'bounds',[0;0],[10;10],'int',1);
miqp_sval(i) = -11.5;
i = i + 1;

%MIQP 2
H = eye(3);
f = -[2 3 1]';
A = [1 1 1;3 -2 -3; 1 -3 2];
b = [1;1;1];
miqp_tprob{i} = optiprob('hess',H,'grad',f,'ineq',A,b,'int','CIC');
miqp_sval(i) = -2.75;
i = i + 1;

clear H f A b e Aeq beq lb ub i no
save 'Utilities/Install/Test Results/miqp_test_results.mat'

%% SDP Programming
clc
clear
no = 5;
sdp_tprob = cell(no,1);
sdp_sval = zeros(no,1);
i = 1;

%SDP1
f = 1;
A = eye(2);
C = -[0 sqrt(2); sqrt(2) 0];
sdp = sparse([C(:) A(:)]);
sdp_tprob{i} = optiprob('f',f,'sdcone',sdp);
sdp_sval(i) = sqrt(2);
i = i + 1;
sdp = [];

%SDP2
f = [1 1]';
C = -[0 2; 2 0];
A0 = [1 0; 0 0];
A1 = [0 0; 0 1];
sdp = sparse([C(:) A0(:) A1(:)]);
lb = [0;0];
ub = [10;10];
sdp_tprob{i} = optiprob('f',f,'sdcone',sdp,'bounds',lb,ub);
sdp_sval(i) = 4;
i = i + 1;
sdp = [];

%SDP3
f = [1 0 0 0]';
%SDP Constraint1 [x2 x3;x3 x4] <= x1*eye(2)
C = zeros(2);
A0 = eye(2);
A1 = -[1 0; 0 0];
A2 = -[0 1; 1 0];
A3 = -[0 0; 0 1];
sdp{1} = sparse([C(:) A0(:) A1(:) A2(:) A3(:)]);
%SDP Constraint2 [x2 x3; x3 x4] >= [1 0.2; 0.2 1]
C = [1 0.2; 0.2 1];
A0 = zeros(2);
A1 = [1 0; 0 0];
A2 = [0 1; 1 0];
A3 = [0 0; 0 1];
sdp{2} = sparse([C(:) A0(:) A1(:) A2(:) A3(:)]);
sdp_tprob{i} = optiprob('f',f,'sdcone',sdp);
sdp_sval(i) = 1.2;
i = i + 1;
sdp = [];

%SDP4
f = [1 1 1]';
C = -[0 1 2; 1 0 3; 2 3 100];
A0 = [1 0 0; 0 0 0; 0 0 0];
A1 = [0 0 0; 0 1 0; 0 0 0];
A2 = zeros(3);
sdp = sparse([C(:) A0(:) A1(:) A2(:)]);
lb = [10;0;0];
ub = [1000;1000;1000];
sdp_tprob{i} = optiprob('f',f,'sdcone',sdp,'bounds',lb,ub);
sdp_sval(i) = 10.1787148490962;
i = i + 1;
sdp = [];

%SDP5
f = -[1 2];
%SDP Constraint 1
C = [2 1; 1 2];
A0 = -[3 1; 1 3];
A1 = -zeros(2);
sdp{1} = sparse([C(:) A0(:) A1(:)]);
%SDP Constraint 2
C = [3 0 1; 0 2 0; 1 0 3];
A0 = -zeros(3);
A1 = -[3 0 1; 0 4 0; 1 0 5];
sdp{2} = sparse([C(:) A0(:) A1(:)]);
%SDP Constraint 3
C = zeros(2);
A0 = -[1 0; 0 0];
A1 = -[0 0; 0 1];
sdp{3} = sparse([C(:) A0(:) A1(:)]);
sdp_tprob{i} = optiprob('f',f,'sdcone',sdp);
sdp_sval(i) = 2.74999999779937;

clear f A0 A1 A2 A3 sdp lb ub i no
save 'Utilities/Install/Test Results/sdp_test_results.mat'

%% Nonlinear Least Squares
clc
clear
no = 5;
nls_tprob = cell(no,1);
nls_sval = zeros(no,1);
i = 1;

%NLS 1
fun = @(x) [10*(x(2)-x(1)^2); 1 - x(1)];
ydata = zeros(2,1);
x0 = [-1.2;1];
nls_tprob{i} = optiprob('fun',fun,'ydata',ydata,'x0',x0);
nls_sval(i) = 0;
i = i + 1;

%NLS 2
fun = @(x) [1.5 - x(1)*(1 - x(2)); 2.25 - x(1)*(1 - x(2)^2); 2.625 - x(1)*(1 - x(2)^3) ];
ydata = zeros(3,1);
x0 = [1;1];
nls_tprob{i} = optiprob('fun',fun,'ydata',ydata,'x0',x0);
nls_sval(i) = 0;
i = i + 1;

%NLS 3
fun = @(x) [1e4*x(1)*x(2) - 1;
            exp(-x(1)) + exp(-x(2)) - 1.0001];
ydata = zeros(2,1);
x0 = [0;1];
nls_tprob{i} = optiprob('fun',fun,'ydata',ydata,'x0',x0);
nls_sval(i) = 0;
i = i + 1;

%NLS 4
ii = (1:10)';
fun = @(x) 2 + 2*ii - (exp(0.2578*ii) + exp(0.2578*ii));
ydata = zeros(10,1);
x0 = [0.3;0.4];
nls_tprob{i} = optiprob('fun',fun,'ydata',ydata,'x0',x0);
nls_sval(i) = 124.36226865;
i = i + 1;

%NLS 5
fun = @(x) [10*(x(2)-x(1)^2);
            1 - x(1);
            sqrt(90)*(x(4)-x(3)^2);
            1 - x(3);
            sqrt(10)*(x(2) + x(4) - 2);
            10^(-0.5)*(x(2)-x(4))];
x0 = [3;-1;-3;-1];
ydata = zeros(6,1);
nls_tprob{i} = optiprob('fun',fun,'ydata',ydata,'x0',x0);
nls_sval(i) = 0;

clear fun ydata x0 ii i no
save 'Utilities/Install/Test Results/nls_test_results.mat'

%% Nonlinear Programming
clc
clear
no = 3;
nlp_tprob = cell(no,1);
nlp_sval = zeros(no,1);
i = 1;

%NLP 1
fun = @(x) 100*(x(2) - x(1)^2)^2 + (1 - x(1))^2;
grad = @(x)[[2*x(1)-400*x(1)*(-x(1)^2+x(2))-2];[-200*x(1)^2+200*x(2)]];
lb = [-inf; -1.5];
ub = [100; 100];
x0 = [-2; 1];
nlp_tprob{i} = optiprob('obj',fun,'grad',grad,'bounds',lb,ub,'x0',x0);
nlp_sval(i) = 0;
i = i + 1;

%NLP 2
fun = @(x) 1/3*(x(1) + 1)^3 + x(2);
grad = @(x)[[(x(1)+1)^2];[1]];
lb = [1;0];
ub = [100;100];
x0 = [1.125;0.125];
nlp_tprob{i} = optiprob('obj',fun,'grad',grad,'bounds',lb,ub,'x0',x0);
nlp_sval(i) = 8/3;
i = i + 1;

%NLP 3
fun = @(x) sin(x(1) + x(2)) + (x(1) - x(2))^2 - 1.5*x(1) + 2.5*x(2) + 1;
grad = @(x)[[2*x(1)-2*x(2)+cos(x(1)+x(2))-1.5];[2*x(2)-2*x(1)+cos(x(1)+x(2))+2.5]];
lb = [-1.5;-3];
ub = [4;3];
x0 = [0;0];
nlp_tprob{i} = optiprob('obj',fun,'grad',grad,'bounds',lb,ub,'x0',x0);
nlp_sval(i) = -0.5*sqrt(3)-pi/3;
i = i + 1;

clear fun grad lb ub x0 i no
save 'Utilities/Install/Test Results/nlp_test_results.mat'

%% MI Nonlinear Programming
clc
clear
no = 1;
minlp_tprob = cell(no,1);
minlp_sval = zeros(no,1);
i = 1;

%MINLP 1
obj = @(x) (x(1) - 5)^2 + x(2)^2 - 25;
nlcon = @(x) -x(1)^2 + x(2)-0.5;
nlrhs = 0;
nle = 1;
x0 = [4;0];
minlp_tprob{i} = optiprob('obj',obj,'ndec',2,'ivars',[1 2],'nlmix',nlcon,nlrhs,nle,'x0',x0);
minlp_sval(i) = -5;
i = i + 1;

clear obj A b nlcon nlrhs nle lb ub x0 i no
save 'Utilities/Install/Test Results/minlp_test_results.mat'


function [r]=RMSE(err)
% r = RMSE(err)
% author: Liang Xiong (lxiong@cs.cmu.edu)

r = sqrt(mean(err(:).^2));


%% KalmanFilterX Demo
% ----------------------
% * This script demonstrates the process of configuring an running a
%   KalmanFilterX object to perform single-target state estimation.
%
% * A toy single-target scenario is considered, for a target that generates
%   regular reports of it's position in the absence of clutter and/or
%   missed detection.
%
%% Extract the GroundTruth data from the example workspace
load('single-target-tracking.mat');
NumIter = size(TrueTrack.Trajectory,2);

%% Models
% Instantiate a Transitionamic model
transition_model = ConstantVelocityX('NumDims',2,'VelocityErrVariance',0.0001);

% Instantiate an Observation model
measurement_model = LinearGaussianX('NumMeasDims',2,'NumStateDims',4,'MeasurementErrVariance',0.02,'Mapping',[1 3]);
%measurement_model = RangeBearing2CartesianX('NumStateDims',4,'MeasurementErrVariance',[0.001,0.02],'Mapping',[1 3]);

% Compile the State-Space model
model = StateSpaceModelX(transition_model,measurement_model);

%% Simulation
% Data Simulator
dataSim = SingleTargetMeasurementSimulatorX(model);

% Simulate some measurements from ground-truth data
MeasurementScans = dataSim.simulate(TrueTrack);
measurements = [MeasurementScans.Vectors];

%% Initiation
% Use the first measurement scan to perform single-point initiation
measurement = MeasurementScans(1).Measurements;
timestamp = measurement.Timestamp;

% Setup prior
xPrior = measurement.Model.Measurement.finv(measurement.Vector);
PPrior = 10*transition_model.covar();
dist = GaussianDistributionX(xPrior,PPrior);
StatePrior = GaussianStateX(dist,timestamp);

% Initiate a track using the generated prior
track = TrackX(StatePrior, TagX(1));

%% Estimation
% Instantiate a filter objects
filter = KalmanFilterX('Model',model);
figure;
for t = 2:NumIter

    % Provide filter with the new measurement
    MeasurementList = MeasurementScans(t);

    % Perform filtering
    prior = track.State;
    prediction = filter.predict(prior, MeasurementList.Timestamp);
    posterior = filter.update(prediction, MeasurementList);

    % Log the data
    track.Trajectory(end+1) = posterior;

    clf;
    hold on;
    meas = measurement_model.finv(measurements(:,1:t));
    true_means = [TrueTrack.Trajectory(1:t).Vector];
    track_means = [track.Trajectory(1:t).Mean];
    plot(true_means(1,1:t), true_means(3,1:t),'.-k', track_means(1,1:t), track_means(3,1:t), 'b-', meas(1,:), meas(3,:), 'rx');
    plotgaussellipse(track.Trajectory(t).Mean([1,3],1),...
                     track.Trajectory(t).Covar([1,3],[1,3]));
    legend('GroundTrouth','Estimated Mean','Measurements', 'Estimated Covariance');
    xlabel("x coordinate (m)");
    ylabel("y coordinate (m)");
    axis([2 9 1 9]);
    drawnow();
end


function [normal,edgeLength,unitNormal] = edgenormal(node,edge)
% compute normal vector of a triangular face

edgeVec = node(edge(:,2),:)-node(edge(:,1),:);
normal = [edgeVec(:,2), -edgeVec(:,1)];
if nargout > 1
    edgeLength = sqrt(sum(normal.^2,2));
    if nargout > 2
        unitNormal = normal./repmat(edgeLength,[1,2]);
    end
end


function [FrontNo,MaxFNo] = NDSort_SDR(PopObj,nSort)
% Do non-dominated sorting by strengthened dominance relation (SDR)

%------------------------------- 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".
%--------------------------------------------------------------------------

    N      = size(PopObj,1);
    NormP  = sum(PopObj,2);
    cosine = 1 - pdist2(PopObj,PopObj,'cosine');
    cosine(logical(eye(length(cosine)))) = 0;
    Angle  = acos(cosine);

    temp  = sort(unique(min(Angle,[],2)));
    minA  = temp(min(ceil(N/2),end));
    Theta = max(1,(Angle./minA).^1);

    dominate = false(N);
    for i = 1 : N-1
        for j = i+1 : N
            if NormP(i)*Theta(i,j) < NormP(j)
                dominate(i,j) = true;
            elseif NormP(j)*Theta(j,i) < NormP(i)
                dominate(j,i) = true;
            end
        end
    end

    FrontNo = inf(1,N);
    MaxFNo  = 0;
    while sum(FrontNo~=inf) < min(nSort,N)
        MaxFNo  = MaxFNo + 1;
        current = ~any(dominate,1) & FrontNo==inf;
        FrontNo(current)    = MaxFNo;
        dominate(current,:) = false;
    end
end


function c = power(a,b)
% POWER implements a.^b, where either a or bor both is an adiff object.

switch [class(a),class(b)]

case 'adiffdouble'
   c = adiff( a.x.^b, rowmult(b.*a.x.^(b-1), a.dx), a.root);

case 'doubleadiff'
   c = adiff( a.^b.x, rowmult(a.^b.x.*log(a), b.dx), b.root);

case 'adiffadiff'
   c = exp(log(a).*b);
   % Commented out code does it the hard way:
   % cx  = a.x.^b.x;
   % cdx = rowmult(cx.*log(a.x), b.dx) + rowmult(a.x.^(b.x-1), a.dx);
   % c   = class(struct('x',cx,'dx',cdx),'adiff');

otherwise
   error(['Can''t do ',class(a),'.^',class(b)]);

end


function [App,Dis] = subFitness(PopObj,P,Center,R)
% Update intersection point solution in each subregion

%------------------------------- 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".
%--------------------------------------------------------------------------

    K          = length(R);
    [N,M]      = size(PopObj);

    % Normalize the population
    fmin       = min(PopObj,[],1);
    fmax       = max(PopObj,[],1);
    Obj        = (PopObj-repmat(fmin,N,1))./repmat(fmax-fmin,N,1);

    %% Calculate intersection point in each subregion
    if K == 1
        InterPoint = interPoint(Obj,P);
    else
        InterPoint = ones(N,M);

        % Allocation
        transformation = Allocation(Obj,Center,R);

        for i = 1 : K
            current     = find(transformation == i);
            if ~isempty(current)
                sInterPoint = interPoint(Obj(current,:),P(i,:));
                InterPoint(current,:) = sInterPoint;
            end
        end
    end

    % Calculate the diversity and convergence of intersection points
    App = min(InterPoint-Obj,[],2);
%     App = sqrt(sum(InterPoint.^2,2)) - sqrt(sum(Obj.^2,2));

    % Calculate the diversity of each solution
%     Dis = pdist2(InterPoint,InterPoint);
    Dis = distMax(InterPoint);
    Dis(logical(eye(length(Dis)))) = inf;
end

function InterPoint = interPoint(PopObj,P)
% Calcualte the approximation degree of each solution, and the distances
% between the intersection points of the solutions

    [N,~] = size(PopObj);

    %% Calculate the intersections by gradient descent
    P     = repmat(P,N,1);      % Powers
    r     = ones(N,1);          % Parameters to be optimized
    lamda = zeros(N,1) + 0.002;   % Learning rates
    E     = sum((r.*PopObj).^P,2) - 1;   % errors
    for i = 1 : 1000
        newr = r - lamda.*E.*sum(P.*PopObj.^P.*r.^(P-1),2);
        newE = sum((newr.*PopObj).^P,2) - 1;
        update         = newr > 0 &sum(newE.^2) < sum(E.^2);
        r(update)      = newr(update);
        E(update)      = newE(update);
        lamda(update)  = lamda(update)*1.002;
        lamda(~update) = lamda(~update)/1.002;
    end
    InterPoint = PopObj.*r;
end

function Dis = distMax(X)
% distMax pairwise distance between one set of observations
% Dis = distMax(X) returns a matrix D containing the maximum absolute
%   distance per dimension between each pair of observations in the MX-by-N
%   data matrix X and MX-by-N data matrix X.

%   Example:
%      X = randn(100, 5);
%      D = distMax(X,Y);
%   >>size(D) = 100*100

    if isempty(X)
        error('X must be a non-empty matrix');
    end

    [N,~] = size(X); % nx,p
    Dis = zeros(N,N);
    for i = 1 : N
        for j = i+1 : N
            Dis(i,j) = max(abs(X(i,:)-X(j,:)));
        end
    end
    Dis = Dis + Dis';
end


function [x, c, funVal, ValueL]=tree_mcLogisticR(A, y, z, opts)
%
%%
% Function mcLogisticR:
%      Logistic Loss for Multi-class (task) Learning
%             with the tree structured group Lasso Regularization
%
%% Problem
%
%  min  - sum_{il} weight_{il} log( p_{il} ) + z * sum_i sum_j w_j ||x^i_{G_j}||
%
%  p_{il}= 1 / (1+ exp(-y_i (x_i' * a_i + c_l) ) ) denotes the probability
%  weight_{il} is the weight for the i-th sample in the l-th classifier
%                is a m x k matrix
%  c_l is the intercept for the l-th classfier, and is a 1xk vector
%  x_i denotes the i-th column of x
%  x^j denotes the j-th row of x
%  a_i' denotes the i-th row of A
%
%  y_i (either 1 or -1) is the response
%
%  In this implementation, we assume weight_{il}=1/(mk)
%
%  x^i denotes the i-th row of x
%
%  G_j's are nodes with tree structure
%
%  The tree structured group information is contained in
%  opts.ind, which is a 3 x nodes matrix, where nodes denotes the number of
%  nodes of the tree.
%
%  opts.ind(1,:) contains the starting index
%  opts.ind(2,:) contains the ending index
%  opts.ind(3,:) contains the corresponding weight (w_j)
%
%  Note:
%  1) If each element of x^j is a leaf node of the tree and the weight for
%  this leaf node are the same, we provide an alternative "efficient" input
%  for this kind of node, by creating a "super node" with
%  opts.ind(1,1)=-1; opts.ind(2,1)=-1; and opts.ind(3,1)=the common weight.
%
%  2) If the features are well ordered in that, the features of the left
%  tree is always less than those of the right tree, opts.ind(1,:) and
%  opts.ind(2,:) contain the "real" starting and ending indices. That is to
%  say, x^j( opts.ind(1,j):opts.ind(2,j) ) denotes x^j_{G_j}. In this case,
%  the entries in opts.ind(1:2,:) are within 1 and k (the number of tasks).
%
%
%  If the features are not well ordered, please use the input opts.G for
%  specifying the index so that
%   x^j( opts.G ( opts.ind(1,j):opts.ind(2,j) ) ) denotes x^j_{G_j}.
%  In this case, the entries of opts.G are within 1 and n, and the entries of
%  opts.ind(1:2,:) are within 1 and length(opts.G).
%
% The following example shows how G and ind works:
%
% G={ {1, 2}, {4, 5}, {3, 6}, {7, 8},
%     {1, 2, 3, 6}, {4, 5, 7, 8},
%     {1, 2, 3, 4, 5, 6, 7, 8} }.
%
% ind={ [1, 2, 100]', [3, 4, 100]', [5, 6, 100]', [7, 8, 100]',
%       [9, 12, 100]', [13, 16, 100]', [17, 24, 100]' },
%
% where each node has a weight of 100.
%
%% Input parameters:
%
%  A-         Matrix of size m x n
%                A can be a dense matrix
%                         a sparse matrix
%                         or a DCT matrix
%  y -        Response vector (of size m x k)
%  z -        Regularization parameter (z >=0)
%  opts-      optional inputs (default value: opts=[])
%
%% Output parameters:
%  x-         The obtained weight of size n x k
%  c-         The obtained intercept if size 1 x k
%  funVal-    Function value during iterations
%
%% Copyright (C) 2009-2010 Jun Liu, and Jieping Ye
%
% You are suggested to first read the Manual.
%
% For any problem, please contact with Jun Liu via j.liu@asu.edu
%
% Last modified on October 3, 2010.
%
%% Related papers
%
% [1] Jun Liu and Jieping Ye, Moreau-Yosida Regularization for
%     Grouped Tree Structure Learning, NIPS 2010
%
%% Related functions:
%
%  sll_opts
%
%%

%% Verify and initialize the parameters
%%
if (nargin <4)
    error('\n Inputs: A, y, z, and opts (.ind) should be specified!\n');
end

[m,n]=size(A);

k=size(y,2);

if (size(y,1) ~=m)
    error('\n Check the length of y!\n');
end

if (z<0)
    error('\n z should be nonnegative!\n');
end

opts=sll_opts(opts); % run sll_opts to set default values (flags)

% restart the program for better efficiency
%  this is a newly added function
if (~isfield(opts,'rStartNum'))
    opts.rStartNum=opts.maxIter;
else
    if (opts.rStartNum<=0)
        opts.rStartNum=opts.maxIter;
    end
end

%% Detailed initialization
%% Normalization

% Please refer to sll_opts for the definitions of mu, nu and nFlag
%
% If .nFlag =1, the input matrix A is normalized to
%                     A= ( A- repmat(mu, m,1) ) * diag(nu)^{-1}
%
% If .nFlag =2, the input matrix A is normalized to
%                     A= diag(nu)^{-1} * ( A- repmat(mu, m,1) )
%
% Such normalization is done implicitly
%     This implicit normalization is suggested for the sparse matrix
%                                    but not for the dense matrix
%

if (opts.nFlag~=0)
    if (isfield(opts,'mu'))
        mu=opts.mu;
        if(size(mu,2)~=n)
            error('\n Check the input .mu');
        end
    else
        mu=mean(A,1);
    end

    if (opts.nFlag==1)
        if (isfield(opts,'nu'))
            nu=opts.nu;
            if(size(nu,1)~=n)
                error('\n Check the input .nu!');
            end
        else
            nu=(sum(A.^2,1)/m).^(0.5); nu=nu';
        end
    else % .nFlag=2
        if (isfield(opts,'nu'))
            nu=opts.nu;
            if(size(nu,1)~=m)
                error('\n Check the input .nu!');
            end
        else
            nu=(sum(A.^2,2)/n).^(0.5);
        end
    end

    ind_zero=find(abs(nu)<= 1e-10);    nu(ind_zero)=1;
    % If some values in nu is typically small, it might be that,
    % the entries in a given row or column in A are all close to zero.
    % For numerical stability, we set the corresponding value to 1.
end

if (~issparse(A)) && (opts.nFlag~=0)
    fprintf('\n -----------------------------------------------------');
    fprintf('\n The data is not sparse or not stored in sparse format');
    fprintf('\n The code still works.');
    fprintf('\n But we suggest you to normalize the data directly,');
    fprintf('\n for achieving better efficiency.');
    fprintf('\n -----------------------------------------------------');
end

%% Group and others

% Initialize ind
if (~isfield(opts,'ind'))
    error('\n In tree_mcLeastR, the field .ind should be specified');
else
    ind=opts.ind;

    if (size(ind,1)~=3)
        error('\n Check opts.ind');
    end
end

GFlag=0;
% if GFlag=1, we shall apply general_altra
if (isfield(opts,'G'))
    GFlag=1;

    G=opts.G;
    if (max(G) >k || max(G) <1)
        error('\n The input G is incorrect. It should be within %d and %d',1,k);
    end
end

%% Starting point initialization

% % % compute AT y
% % if (opts.nFlag==0)
% %     ATy =A'* y;
% % elseif (opts.nFlag==1)
% %     ATy= A'* y -  mu' * sum(y, 1);    ATy=ATy./repmat(nu, 1, k);
% % else
% %     invNu=y./repmat(nu, 1, k);        ATy=A'*invNu-mu' * sum(invNu, 1);
% % end

p_flag=(y==1);                  % the indices of the postive samples
m1=sum(p_flag,1);               % the total number of the positive samples
m2=m-m1;                        % the total number of the negative samples
% m1 and m2 are 1 x k vectors

% process the regularization parameter
if (opts.rFlag==0)
    lambda=z;
else % z here is the scaling factor lying in [0,1]
%     if (z<0 || z>1)
%         error('\n opts.rFlag=1, and z should be in [0,1]');
%     end

    % we compute ATb for computing lambda_max, when the input z is a ratio

    computedFlag=0;
    if (isfield(opts,'lambdaMax'))
        if (opts.lambdaMax~=-1)
            lambda=z*opts.lambdaMax;
            computedFlag=1;
        end
    end

    if (~computedFlag)

        b=zeros(m, k);                  % b is of size m x k
        b= b + p_flag.*repmat(m2,m,1);
        b= b -(~p_flag).*repmat(m1,m,1);
        b=b/m;

        % compute AT b
        if (opts.nFlag==0)
            ATb =A'* b;
        elseif (opts.nFlag==1)
            ATb= A'* b -  mu' * sum(b, 1);    ATb=ATb./repmat(nu, 1, k);
        else
            invNu=b./repmat(nu, 1, k);        ATb=A'*invNu-mu' * sum(invNu, 1);
        end

        if (GFlag==0)
            lambda_max=findLambdaMax_mt(ATb, n, k, ind, size(ind,2));
        else
            lambda_max=general_findLambdaMax_mt(ATb, n, k, G, ind, size(ind,2));
        end

        lambda_max= lambda_max / (m*k);

        % As .rFlag=1, we set lambda as a ratio of lambda_max
        lambda=z*lambda_max;
    end
end

% The following is for computing lambdaMax
% we use opts.lambdaMax=-1 to show that we need the computation.
%
% One can use this for setting up opts.lambdaMax
if (isfield(opts,'lambdaMax'))

    if (opts.lambdaMax==-1)

        b=zeros(m, k);                  % b is of size m x k
        b= b + p_flag.*repmat(m2,m,1);
        b= b -(~p_flag).*repmat(m1,m,1);
        b=b/m;

        % compute AT b
        if (opts.nFlag==0)
            ATb =A'* b;
        elseif (opts.nFlag==1)
            ATb= A'* b -  mu' * sum(b, 1);    ATb=ATb./repmat(nu, 1, k);
        else
            invNu=b./repmat(nu, 1, k);        ATb=A'*invNu-mu' * sum(invNu, 1);
        end

        % compute lambda_max
        if (GFlag==0)
            lambda_max=findLambdaMax_mt(ATb, n, k, ind, size(ind,2));
        else
            lambda_max=general_findLambdaMax_mt(ATb, n, k, G, ind, size(ind,2));
        end

        x=lambda_max;
        funVal=lambda_max;
        ValueL=lambda_max;

        return;
    end
end

% initialize a starting point
if opts.init==2
    x=zeros(n,k); c=log(m1./m2);
else
    if isfield(opts,'x0')
        x=opts.x0;
        if ( size(x,1)~=n || size(x,2)~=k )
            error('\n Check the input .x0');
        end
    else
        x=zeros(n,k);
    end

    if isfield(opts,'c0')
        c=opts.c0;

        if ( length(c)~=k )
            error('\n Check the input .c0');
        end
    else
        c=log(m1./m2);
    end
end

% compute Ax= A * x
if (opts.nFlag==0)
    Ax=A* x;
elseif (opts.nFlag==1)
    invNu=x./repmat(nu, 1, k);  mu_invNu=mu * invNu;
    Ax=A*invNu -repmat(mu_invNu, m, 1);
else
    Ax=A*x-repmat(mu*x, m, 1);     Ax=Ax./repmat(nu, 1, k);
end

%% The main program

if (opts.mFlag==0 && opts.lFlag==0)
    % The Armijo Goldstein line search schemes + accelearted gradient descent

    bFlag=0; % this flag tests whether the gradient step only changes a little

    L=1/(m*k); % the intial guess of the Lipschitz continuous gradient

    % assign xp with x, and Axp with Ax
    xp=x; Axp=Ax; xxp=zeros(n,k);
    cp=c;         ccp=zeros(1,k);

    alphap=0; alpha=1;

    for iterStep=1:opts.maxIter
        % --------------------------- step 1 ---------------------------
        % compute search point s based on xp and x (with beta)
        beta=(alphap-1)/alpha;    s=x + beta* xxp;  sc=c + beta* ccp;

        % --------------------------- step 2 ---------------------------
        % line search for L and compute the new approximate solution x

        % compute the gradient (g) at s
        As=Ax + beta* (Ax-Axp);

        % aa= - diag(y) * (A * s + sc)
        aa=- y.*(As+ repmat(sc,m,1));

        % fun_s is the logistic loss at the search point
        bb=max(aa,0);
        fun_s= sum(sum ( log( exp(-bb) +  exp(aa-bb) ) + bb ) ) / (m * k);

        % compute prob=[p_1;p_2;...;p_m]
        prob=1./( 1+ exp(aa) );

        % b= - diag(y) * (1 - prob)
        b= -y.*(1-prob) / (m * k);

        gc=sum(b); % the gradient of c

        % compute g= AT b, the gradient of x
        if (opts.nFlag==0)
            g=A'*b;
        elseif (opts.nFlag==1)
            g= A'* b -  mu' * sum(b, 1);  g=g./repmat(nu, 1, k);
        else
            invNu=b./repmat(nu, 1, k);        g=A'*invNu-mu' * sum(invNu, 1);
        end

        % copy x and Ax to xp and Axp
        xp=x;    Axp=Ax;
        cp=c;

        while (1)
            % let s walk in a step in the antigradient of s to get v
            % and then do the L1/Lq-norm regularized projection
            v=s-g/L; c= sc- gc/L;

            % tree overlapping group Lasso projection
            ind_work(1:2,:)=ind(1:2,:);
            ind_work(3,:)=ind(3,:) * (lambda / L);

            if (GFlag==0)
                x=altra_mt(v, n, k, ind_work, size(ind_work,2));
            else
                x=general_altra_mt(v, n, k, G, ind_work, size(ind_work,2));
            end

            v=x-s;  % the difference between the new approximate solution x
            % and the search point s

            % compute Ax= A * x
            if (opts.nFlag==0)
                Ax=A* x;
            elseif (opts.nFlag==1)
                invNu=x./repmat(nu, 1, k);  mu_invNu=mu * invNu;
                Ax=A*invNu -repmat(mu_invNu, m, 1);
            else
                Ax=A*x-repmat(mu*x, m, 1);     Ax=Ax./repmat(nu, 1, k);
            end

            % aa= - diag(y) * (A * x + c)
            aa=- y.*(Ax+ repmat(c,m,1));

            % fun_x is the logistic loss at the new approximate solution
            bb=max(aa,0);
            fun_x= sum(sum ( log( exp(-bb) +  exp(aa-bb) ) + bb ) ) / (m * k);

            r_sum=(norm(v,'fro')^2 + norm(c-sc,2)^2) / 2;
            l_sum=fun_x - fun_s - sum(sum(v.* g)) - (c-sc)* gc';

            if (r_sum <=1e-20)
                bFlag=1; % this shows that, the gradient step makes little improvement
                break;
            end

            % the condition is fun_x <= fun_s + <v, g> + <c ,gc>
            %                           + L/2 * (<v,v> + <c-sc,c-sc> )
            if(l_sum <= r_sum * L)
                break;
            else
                L=max(2*L, l_sum/r_sum);
                % fprintf('\n L=%5.6f',L);
            end
        end

        % --------------------------- step 3 ---------------------------
        % update alpha and alphap, and check whether converge
        alphap=alpha; alpha= (1+ sqrt(4*alpha*alpha +1))/2;

        ValueL(iterStep)=L;

        xxp=x-xp;  ccp=c-cp;

        funVal(iterStep)=fun_x;

        for i=1:n
            xRow=x(i,:);

            if (GFlag==0)
                tree_norm=treeNorm(xRow, k, ind, size(ind,2));
            else
                tree_norm=general_treeNorm(xRow, k, G, ind, size(ind,2));
            end

            funVal(iterStep)=funVal(iterStep)+ lambda*tree_norm;
        end

        if (bFlag)
            % fprintf('\n The program terminates as the gradient step changes the solution very small.');
            break;
        end

        switch(opts.tFlag)
            case 0
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <= opts.tol)
                        break;
                    end
                end
            case 1
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <=...
                            opts.tol* funVal(iterStep-1))
                        break;
                    end
                end
            case 2
                if ( funVal(iterStep)<= opts.tol)
                    break;
                end
            case 3
                norm_xxp=norm(xxp,'fro');
                if ( norm_xxp <=opts.tol)
                    break;
                end
            case 4
                norm_xp=norm(xp,'fro');    norm_xxp=norm(xxp,'fro');
                if ( norm_xxp <=opts.tol * max(norm_xp,1))
                    break;
                end
            case 5
                if iterStep>=opts.maxIter
                    break;
                end
        end
    end
else
    error('\n The function does not support opts.mFlag neq 0 & opts.lFlag neq 0!');
end


%  Figure 10.60      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
% script for disk read/write head control design
% the design is based on 1/s^2 with robustness
% with respect to a resonance of damping zeta = z at wm.
% the design calls for a single lead selected to maximize wc
% the phase margin is set by alpha = a
% the compensator has a second order roll-off filter
% the resonance is 'gain stabilized' with GM
% the time is scaled to milliseconds
wm= 5*pi
z=.05;
GM = 4;
a=0.1;
% input the nominal plant with a gain of 1.
numGo=1;
denGo= [1 0 0];
sysGo=tf(numGo,denGo);
% input the plant with the resonance
numG= [1/(50*pi) 1];
denG=[1/(25*pi^2) 1/(50*pi) 1  0 0];
sysG=tf(numG,denG);
% Define the design constant
A = ((a*2*z)/(GM))^.333;
% PM >50; we have selected alpha = a = 0.1
Tinv= A*wm*sqrt(a);
T=1/Tinv;
wc =  A*wm
% K =  wc^2;
K=  wc^2;
% Input the lead compensation
numD =K*sqrt(a)*[T 1];
denD = [a*T 1];
sysD=tf(numD,denD);
% Now design the roll-off filter
w1=wm*sqrt(A/sqrt(a));
% design the roll-off damping
z1=.3 ;
numF=1;
denF=[1/w1^2  2*z1/w1 1];
sysF=tf(numF,denF);
% now compute the entire compensation
sysDc=sysD*sysF;
sysOLo=sysD*sysGo;
sysOL=sysDc*sysG;
% bode(sysOLo);
% hold on
clf;
sysH=tf(1,1);
sysCL=feedback(sysOL,sysH);
t=0:0.1:6;
[y,t]=step(sysCL,t);
plot(t,y);
xlabel('Time (msec)');
ylabel('Amplitude');
grid on
title('Fig. 10.60 Step response of disk control;lead plus roll-off ')


classdef Quadrature_Triangle < Quadrature

    methods
        function computeQuadrature(obj,order)
            computeQuadrature@Quadrature(obj,order);
            switch order
                case 'CONSTANT'
                    obj.ngaus = 1;
                    obj.weigp = 1/2;
                    obj.posgp = [1/3;1/3];

                case 'LINEAR'
                    obj.ngaus = 1;
                    obj.weigp = [1/2];
                    obj.posgp = [1/3;1/3];

                case 'QUADRATIC'
                    %obj.ngaus = 3;
                    %obj.weigp = [1/6;1/6;1/6];
                    %obj.posgp = [0,0.5;0.5,0;0.5,0.5]';
                    obj.ngaus = 3;
                    obj.weigp = [1/6;1/6;1/6];
                    obj.posgp = [2/3,1/6;1/6,1/6;1/6,2/3]';
                case 'CUBIC'
                    obj.ngaus = 4;
                    obj.weigp = [-27/96;25/96;25/96;25/96];
                    obj.posgp = [1/3,1/3;1/5,1/5;3/5,1/5;1/5,3/5]';

                case 'QUADRATIC2'
                    obj.ngaus = 2;
                    obj.weigp = [1/6;1/6;1/6];
                    obj.posgp = [1/6,1/6;2/3,1/6;1/6,2/3]';

                case 'QUADRATICMASS'
                    obj.ngaus = 3;
                    obj.weigp = [1/6;1/6;1/6];
                    obj.posgp = [2/3,1/6,1/6;1/6,2/3,1/6];
                otherwise
                    disp('Quadrature not implemented for triangle elements')
            end
        end
    end
end


function y = atanh_pos(x,rnd)
% rigorous atanh for nonnegative double vector x<1 rounded corresponding to rnd
% for internal use in rigorous atanh, acoth

% written  12/30/98     S.M. Rump
% modified 08/31/99     S.M. Rump  major revision
%

  y = x;

  % atanh(x) = log( 1 + 2*x/(1-x) ) / 2

  index = ( x<0.33 );                    % 0 <= x < .33
  if any(index)
    X = x(index);
    setround(-rnd)
    E = 1 - X;
    setround(rnd)
    E = 2*X./E;
    y(index) = log_1( E , rnd ) / 2;     % 0 <= E < 1
  end

  index = ~index;
  if any(index)                          % .33 <= x <= 1
    X = x(index);
    setround(-rnd)
    E = 1 - X;
    setround(rnd)
    E = 1 + 2*X./E;
    y(index) = log_rnd( E , rnd ) / 2;
  end


function p_ = EntropyProg(p,A,b,Aeq,beq)
% This function computes the entropy-pooling change of measure, see
% "A. Meucci - Fully Flexible Views: Theory and Practice -
% The Risk Magazine, October 2008, p 97-102"
% available at www.symmys.com > Research > Working Papers

% Code by A. Meucci, November 2008
% Last version available at www.symmys.com > Teaching > MATLAB

K_=size(A,1);
K=size(Aeq,1);
A_=A';
b_=b';
Aeq_=Aeq';
beq_=beq';
x0=zeros(K_+K,1);
InqMat=-eye(K_+K); InqMat(K_+1:end,:)=[];
InqVec=zeros(K_,1);

options = optimset('GradObj','on','Hessian','on');
if ~K_
    v=fminunc(@nestedfunU,x0,options);
    p_=exp(log(p)-1-Aeq_*v);
else
    lv=fmincon(@nestedfunC,x0,InqMat,InqVec,[],[],[],[],[],options);
    l=lv(1:K_);
    v=lv(K_+1:end);
    p_=exp(log(p)-1-A_*l-Aeq_*v);
end

    function [mL g H] = nestedfunU(v)

        x=exp( log(p)-1-Aeq_*v );
        x=max(x,10^(-32));
        L=x'*(log(x)-log(p)+Aeq_*v)-beq_*v;
        mL=-L;

        g = [beq-Aeq*x];
        H = [Aeq*((x*ones(1,K)).*Aeq_)];  % Hessian computed by Chen Qing, Lin Daimin, Meng Yanyan, Wang Weijun
    end

    function [mL g H] = nestedfunC(lv)

        l=lv(1:K_);
        v=lv(K_+1:end);
        x=exp( log(p)-1-A_*l-Aeq_*v );
        x=max(x,10^(-32));
        L=x'*(log(x)-log(p))+l'*(A*x-b)+v'*(Aeq*x-beq);
        mL=-L;

        g = [b-A*x; beq-Aeq*x];
        H = [A*((x*ones(1,K_)).*A_)  A*((x*ones(1,K)).*Aeq_) % Hessian computed by Chen Qing, Lin Daimin, Meng Yanyan, Wang Weijun
            Aeq*((x*ones(1,K_)).*A_)   Aeq*((x*ones(1,K)).*Aeq_)];
    end
end


%% Logistic logistic regression for multiclass classification
clear
k = 3;
n = 1000;
[X,t] = kmeansRnd(2,k,n);
[model, llh] = logitMn(X,t);
y = logitMnPred(model,X);
plotClass(X,y)


function logistic_cdf_values_test ( )

%*****************************************************************************80
%
%% LOGISTIC_CDF_VALUES_TEST demonstrates the use of LOGISTIC_CDF_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 November 2005
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LOGISTIC_CDF_VALUES_TEST:\n' );
  fprintf ( 1, '  LOGISTIC_CDF_VALUES stores values of\n' );
  fprintf ( 1, '  the Logistic Cumulative Density Function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      Mu        Beta          X                  CDF(X)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, mu, beta, x, fx ] = logistic_cdf_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %12f  %12f  %24.16f\n', mu, beta, x, fx );

  end

  return
end


function prime_test ( )

%*****************************************************************************80
%
%% PRIME_TEST tests PRIME.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 December 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PRIME_TEST\n' );
  fprintf ( 1, '  PRIME returns primes from a table.\n' );

  n = -1;
  prime_max = prime ( n );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of primes stored is %d\n', prime_max );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     I    Prime(I)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : 10
    fprintf ( 1, '  %4d  %6d\n', i, prime(i) );
  end
  fprintf ( 1, '\n' );
  for i = prime_max - 10 : prime_max
    fprintf ( 1, '  %4d  %6d\n', i, prime(i) );
  end

  return
end


function [ p, q, pdf ] = normp ( z )

%*****************************************************************************80
%
%% NORMP computes the cumulative density of the standard normal distribution.
%
%  Discussion:
%
%    This is algorithm 5666 from Hart, et al.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 January 2008
%
%  Author:
%
%    Original FORTRAN77 version by Alan Miller.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    John Hart, Ward Cheney, Charles Lawson, Hans Maehly,
%    Charles Mesztenyi, John Rice, Henry Thacher,
%    Christoph Witzgall,
%    Computer Approximations,
%    Wiley, 1968,
%    LC: QA297.C64.
%
%  Parameters:
%
%    Input, real Z, divides the real ( kind = 8 ) line into two
%    semi-infinite intervals, over each of which the standard normal
%    distribution is to be integrated.
%
%    Output, real P, Q, the integrals of the standard normal
%    distribution over the intervals ( - Infinity, Z] and
%    [Z, + Infinity ), respectively.
%
%    Output, real PDF, the value of the standard normal distribution
%    at Z.
%
  cutoff = 7.071;
  p0 = 220.2068679123761;
  p1 = 221.2135961699311;
  p2 = 112.0792914978709;
  p3 = 33.91286607838300;
  p4 = 6.373962203531650;
  p5 = 0.7003830644436881;
  p6 = 0.03526249659989109;
  q0 = 440.4137358247522;
  q1 = 793.8265125199484;
  q2 = 637.3336333788311;
  q3 = 296.5642487796737;
  q4 = 86.78073220294608;
  q5 = 16.06417757920695;
  q6 = 1.755667163182642;
  q7 = 0.08838834764831844;
  root2pi = 2.506628274631001;

  zabs = abs ( z );
%
%  37 < |Z|.
%
  if ( 37.0  < zabs )

    pdf = 0.0;
    p = 0.0;
%
%  |Z| <= 37.
%
  else

    expntl = exp ( - 0.5  * zabs * zabs );
    pdf = expntl / root2pi;
%
%  |Z| < CUTOFF = 10 / sqrt(2).
%
    if ( zabs < cutoff )

      p = expntl * (((((( ...
          p6   * zabs ...
        + p5 ) * zabs ...
        + p4 ) * zabs ...
        + p3 ) * zabs ...
        + p2 ) * zabs ...
        + p1 ) * zabs ...
        + p0 ) / ((((((( ...
          q7   * zabs ...
        + q6 ) * zabs ...
        + q5 ) * zabs ...
        + q4 ) * zabs ...
        + q3 ) * zabs ...
        + q2 ) * zabs ...
        + q1 ) * zabs ...
        + q0 );
%
%  CUTOFF <= |Z|.
%
    else

      p = pdf / ( ...
        zabs + 1.0  / ( ...
        zabs + 2.0  / ( ...
        zabs + 3.0  / ( ...
        zabs + 4.0  / ( ...
        zabs + 0.65  )))));

    end

  end

  if ( z < 0.0  )
    q = 1.0  - p;
  else
    q = p;
    p = 1.0  - q;
  end

  return
end


function linplus_test29 ( )

%*****************************************************************************80
%
%% TEST29 tests R8GE_DET.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  n = 4;
  x = 2.0E+00;
  y = 3.0E+00;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST29\n' );
  fprintf ( 1, '  For a matrix in general storage,\n' );
  fprintf ( 1, '  R8GE_DET computes the determinant.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N = %d\n', n );
%
%  Set the matrix.
%
  for i = 1 : n
    for j = 1 : n
      if ( i == j )
        a(i,j) = x + y;
      else
        a(i,j) = y;
      end
    end
  end
%
%  Factor the matrix.
%
  [ a_lu, pivot, info ] = r8ge_fa ( n, a );
%
%  Compute the determinant.
%
  det = r8ge_det ( n, a_lu, pivot );

  exact = x^(n-1) * ( x + n * y );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  R8GE_DET computes the determinant = %f\n', det );
  fprintf ( 1, '  Exact determinant =                %f\n', exact );

  return
end


% prtUtilSumExp(x)
%   returns log(sum(exp(x),1)) while avoiding underflow issues
%
% Notes: This only sums down, thus the sum( * , 1)
%        This only accepts real doubles
%        If there is a large spread between the min and max values
%           some underflow is still possible. Although unlikely to
%           matter in the end due to the large spread in your values.
%
%        Error checking is minimal and seg faults are possible


%IHOUGH	Hough transform
%
%	params = IHOUGH
%	H = IHOUGH(IM)
%	H = IHOUGH(IM, params)
%
%	Compute the Hough transform of the image IM data.
%
%	The appropriate Hough accumulator cell is incremented by the
%	absolute value of the pixel value if it exceeds
%	params.edgeThresh times the maximum value found.
%
%	Pixels within params.border of the edge will not increment.
%
% 	The accumulator array has theta across the columns and offset down
%	the rows.  Theta spans the range -pi/2 to pi/2 in params.Nth increments.
%	Offset is in the range 1 to number of rows of IM with params.Nd steps.
%
%	Clipping is applied so that only those points lying within the Hough
%	accumulator bounds are updated.
%
%	The output argument H is a structure that contains the accumulator
%	and the theta and offset value vectors for the accumulator columns
%	and rows respectively.  With no output
%	arguments the Hough accumulator is displayed as a greyscale image.
%
%	H.h	the Hough accumulator
%	H.theta	vector of theta values corresponding to H.h columns
%	H.d	vector of offset values corresponding to H.h rows
%
%	For this version of the Hough transform lines are described by
%
%		d = y cos(theta) + x sin(theta)
%
%	where theta is the angle the line makes to horizontal axis, and d is
%	the perpendicular distance between (0,0) and the line.  A horizontal
%	line has theta = 0, a vertical line has theta = pi/2 or -pi/2
%
%	The parameter structure:
%
%	params.Nd number of offset steps (default 64)
%	params.Nth number of theta steps (default 64)
%	params.edgeThresh increment threshold (default 0.1)
%	params.border width of non-incrmenting border(default 8)
%
%
% SEE ALSO: xyhough testpattern isobel ilap

% Copyright (C) 1995-2009, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
%
% MVTB 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.
%
% MVTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.

function H = ihough(IM, params)

	% process arguments
	%  the param structure is common to ihough()
	if nargin < 2,
		default.Nd = 64;
		default.Nth = 64;
		default.edgeThresh = 0.10;
		default.border = 8;

		% for houghpeaks
		default.houghThresh = 0.40;
		default.radius = 5;
		default.interpWidth = 5;
	end
	if nargin == 0,
		H = default;
		return;
	elseif nargin == 1,
		params = default;
	end

	[nr,nc] = size(IM);

	% find the significant edge pixels
	IM = abs(IM);
	globalMax = max(IM(:));
	i = find(IM > (globalMax*params.edgeThresh));
	[r,c]=ind2sub(size(IM), i);

	xyz = [c r IM(i)];

	% eliminate those near the edge
	k = (c < params.border) | (c>(nc-params.border)) | ...
		(r<params.border) | (r>(nr-params.border));
	xyz(k,:) = [];

	% now pass the x/y/strenth info to xyhough
	H = ihough_xy(xyz, [1 nr params.Nd], params.Nth);


%% Image Alignment using ECC algorithm
%
% This sample demonstrates the use of the ECC image alignment algorithm. When
% one image is given, the template image is artificially formed by a random
% warp given the motion type. Otherwise supply both images. If input warp
% matrix is not specified, the identity transformation is used to initialize
% the algorithm.
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/cpp/image_alignment.cpp>
%

%% Options
INPUT_IMAGE = fullfile(mexopencv.root(), 'test', 'fruits.jpg');
LOAD_TEMPLATE = false;
LOAD_MAT = false;
MOTION_TYPE = 'Affine';

%%
% type of motion
MOTION_TYPE = validatestring(MOTION_TYPE, ...
    {'Translation', 'Euclidean', 'Affine', 'Homography'});

%%
% termination criteria (ECC iterations and convergnce epsilon)
CRIT = struct('type','Count+EPS', 'maxCount',50, 'epsilon',1e-4);
if CRIT.maxCount > 200
    warning('Too many iterations');
end

%% Input image
% load input image
imgInput = cv.imread(INPUT_IMAGE, 'Grayscale',true);

%% Template image
% initialize or load template image
M0 = [];
if LOAD_TEMPLATE
    % load an existing template image
    fmts = imformats();
    filtspec = strjoin(strcat('*.', [fmts.ext]), ';');
    [fn,fp] = uigetfile(filtspec, 'Select an image');
    if fp==0, error('No file selected'); end
    imgTemplate = cv.imread(fullfile(fp,fn), 'Grayscale',true);
else
    % create a template image by applying a random warp to input image
    imgInput = cv.resize(imgInput, [216 216]);
    opts = {'DSize',[200 200], 'Interpolation','Linear', 'WarpInverse',true};
    switch MOTION_TYPE
        case 'Translation'
            M0 = [1 0 rand*10+10;
                  0 1 rand*10+10];
            imgTemplate = cv.warpAffine(imgInput, M0, opts{:});
        case 'Euclidean'
            theta = pi/30 + pi*randi([-2 2])/180;
            M0 = [cos(theta) -sin(theta) rand*10+10;
                  sin(theta)  cos(theta) rand*10+10];
            imgTemplate = cv.warpAffine(imgInput, M0, opts{:});
        case 'Affine'
            M0 = [1-(rand*0.1-0.05)    rand*0.06-0.03 rand*10+10;
                     rand*0.06-0.03 1-(rand*0.1-0.05) rand*10+10];
            imgTemplate = cv.warpAffine(imgInput, M0, opts{:});
        case 'Homography'
            M0 = [1-(rand*0.1-0.05)    rand*0.06-0.03 rand*10+10;
                     rand*0.06-0.03 1-(rand*0.1-0.05) rand*10+10;
                     rand*2e-4+2e-4    rand*2e-4+2e-4 1];
            imgTemplate = cv.warpPerspective(imgInput, M0, opts{:});
    end
    display(M0)  % ground truth
end
sz = size(imgTemplate);

%% Input warp matrix
% initialize or load warp matrix
M = [];
if LOAD_MAT
    % load from a MAT-file
    uiopen('load');
    assert(~isempty(M), 'Failed to load warp matrix M');
    if strcmp(MOTION_TYPE, 'Homography')
        assert(isequal(size(M), [3 3]));
    else
        assert(isequal(size(M), [2 3]));
    end
else
    % identity matrix
    if strcmp(MOTION_TYPE, 'Homography')
        M = eye(3,3);
    else
        M = eye(2,3);
    end
    warning(['Performance Warning: Identity warp ideally assumes images ' ...
        'of similar size. If the deformation is strong, the identity ' ...
        'warp may not be a good initialization, and estimation may fail.']);
end
display(M)

%% Estimate transformation
fprintf('Estimating "%s" transformation...\n', MOTION_TYPE);
tic
M = cv.findTransformECC(imgTemplate, imgInput, ...
    'InputWarp',M, 'MotionType',MOTION_TYPE, 'Criteria',CRIT);
toc
display(M)

%%
% compare against ground truth
if ~isempty(M0)
    err = norm(M - M0)
end

%% Apply estimated transformation
% warped image
opts = {'DSize',[sz(2) sz(1)], 'Interpolation','Linear', 'WarpInverse',true};
if strcmp(MOTION_TYPE, 'Homography')
    imgWarped = cv.warpPerspective(imgInput, M, opts{:});
else
    imgWarped = cv.warpAffine(imgInput, M, opts{:});
end

%%
% compare against template image
if ~mexopencv.isOctave() && mexopencv.require('vision')
    imgError = imfuse(imgTemplate, imgWarped, 'diff');
else
    imgError = abs(double(imgTemplate) - double(imgWarped));
    imgError = imgError / max(imgError(:));
end

%%
% compute region boundary in input image corresponding to template image
pts = [1 1; sz(2) 1; sz(2) sz(1); 1 sz(1)]; % corners to warp (TL/TR/BR/BL)
pts(:,3) = 1;  % homogeneous coordinates
if strcmp(MOTION_TYPE, 'Homography')
    pts = pts * M.';
else
    pts = pts * [M; 0 0 1].';
end
pts = bsxfun(@rdivide, pts(:,1:2), pts(:,3));

%%
% show results
subplot(221), imshow(imgInput), title('image')
line(pts([1:end 1],1), pts([1:end 1],2), 'Color','m', 'LineWidth',2)
axis tight
subplot(222), imshow(imgTemplate), title('template')
subplot(223), imshow(imgWarped), title('warped image')
subplot(224), imshow(imgError), title('error')


function ar=lpcls2ar(ls)
%LPCLS2AR convert line spectrum pair frequencies to ar polynomial AR=(LS)
% input vector elements should be in the range 0 to 0.5

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: lpcls2ar.m,v 1.4 2007/05/04 07:01:38 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nf,p]=size(ls);
p1=p+1;
p2 = p1*2;
ar=zeros(nf,p1);
for k=1:nf
  le=exp(ls(k,:)*pi*2i);
  lf=[1 le -1 conj(fliplr(le))];
  y=real(poly(lf(1:2:p2)));
  x=real(poly(lf(2:2:p2)));
  ar(k,:)=(x(1:p1)+y(1:p1))/2;
end


function output_noise=addnoise(sig,input_noise,snr)
noise=input_noise;
noise_std_var=sqrt(10^(-snr/10)*(sig(:)'*sig(:))/(noise(:)'*noise(:)));
output_noise=noise_std_var*noise;
end


function EXP_A = get_RS_matrix_exponential( Q, dt, xi, drifts, vols, psi_J)
N = length(xi);
m_0 = length(drifts);

Qt = dt*Q';
EXP_A = ones(m_0,m_0,N);

drifts = dt*drifts;
vols = (0.5*dt)*vols.^2;

if nargin < 8
    for j = 1:N
        EXP_A(:,:,j) = expm(Qt + diag(drifts*xi(j) - vols*xi(j)^2));  %This incorporates the dt
    end
else
    for j = 1:N
        EXP_A(:,:,j) = expm(Qt + diag(drifts*xi(j) - vols*xi(j)^2 + dt*psi_J(xi(j))));  %This incorporates the dt
    end
end

end


function varargout = chol(varargin)
%CHOL   Cholesky factorization of a DISKFUN.
%
%   R = CHOL( F ), if F is a nonnegative definite DISKFUN then this returns
%   an upper triangular quasimatrix so that R'*R is a decomposition of F.
%   If F is not nonnegative definite then an error is thrown.
%
%   L = CHOL(F, 'lower'), if F is a nonnegative definite DISKFUN then this
%   produces a lower triangular quasimatrix so that L*L' is a decomposition
%   of F. If F is not nonnegative definite then an error is thrown.
%
%   [R, p] = CHOL( F ), with two outputs never throwns an error message. If
%   F is nonnegative definite then p is 0 and R is the same as above. If F
%   is symmetric but negative definite or semidefinite then p is a positive
%   integer such that R has p columns and R'*R is a rank p nonnegative
%   definite DISKFUN that approximates F. This is particular useful when F
%   is nonnegative definite, but rounding error have perturbed it to be
%   semidefinite.
%
%   [L, p] = CHOL(F, 'lower') same as above but the first argument is lower
%   triangular.
%
%   For more information about the factorization:
%   A. Townsend and L. N. Trefethen, Continuous analogues of matrix
%   factorizations, Proc. Royal Soc. A., 2015.
%
% See also DISKFUN/LU, and DISKFUN/QR.

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

% Requires evaluations of f in polar coords once passed to separableApprox,
% so we make a chebfun2:
f = cart2pol(varargin{1}, 'cdr');

[varargout{1:nargout}] = chol@separableApprox(f, varargin{2:end});

end


function filterSteerable( theta )
% Steerable 2D Gaussian derivative filter (for visualization).
%
% This function is a demonstration of steerable filters.  The directional
% derivative of G in an arbitrary direction theta can be found by taking a
% linear combination of the directional derivatives dxG and dyG.
%
% USAGE
%  filterSteerable( theta )
%
% INPUTS
%  theta   - orientation in radians
%
% OUTPUTS
%
% EXAMPLE
%  filterSteerable( pi/4 );
%
% See also filterGauss
%
% Piotr's Image&Video Toolbox      Version 2.0
% Copyright 2012 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Simplified BSD License [see external/bsd.txt]

% Get G
[x,y]=meshgrid(-1:.1:1, -1:.1:1 );
r = sqrt( x.^2 + y.^2 );
G = exp( -r .* r *2 );

% get first derivatives of G.  note: d/dx(G)=-2x*G
phi = atan2( y, x );
dxG = r .* cos(phi) .* G;
dyG = r .* sin(phi) .* G;

% get directional derivative by taking linear comb in theta
Gtheta = cos(theta)*dxG + sin(theta)*dyG;

% dislpay (scale for visualization purposes)
GS = cat(3,G,dxG*2,dyG*2,Gtheta*2);
figure(1); montage2( GS );


function [z_local] = interpolate2local(X_regular,Y_regular,Z_regular,xy_local,int_method)
% [z_local] = interpolate2local(X_regular,Y_regular,Z_regular,xy_local,int_method)
% Interpolates thed ata to a refular grid and fill the gaps outside the convexhull.
% The data is outputed by the new regular grid in X, in Y. The data is a regular grid Z
% with in its thrid dimentsion the the different datasets.
% input:
% X_regular         Regular X-grid in km
% Y_regular         Regular Y_grid in km
% Z_regular         Regular Z-grid (multi-dimensional when having more than 1 dataset)
% int_method  		The interpoaltion method to be used. This is an optional input
%                   argument. By default a triangular interpolation is performed.
% xy_local  		A local xy grid (preferably rotated to reduce interpolation
%                   effects for points outside the convex hull). Needs to be specified
%                   as a 2 column matrix in km.
% output:
% z_local           The data observations that have been interpolated, with each
%                   dataset repressented by a column.
%
%     Copyright (C) 2015  Bekaert David - University of Leeds
%     Email: eedpsb@leeds.ac.uk or davidbekaert.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.,
%     51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
%

% setting the default interpolation method when needed
flag_control_fig = 0;       % when 1 plot the control figures
if nargin < 5
	int_method = [];
end
if isempty(int_method)==1
	int_method = 'linear';
end

% Convert the position grid back to a column vector
x_regular = reshape(X_regular,[],1);
clear X_regular
y_regular = reshape(Y_regular,[],1);
clear Y_regular

% Converting the datagrid back to a column vector or a matrix
n_datasets = size(Z_regular,3);
n_gridpoints = length(y_regular);
z_regular = reshape(Z_regular,n_gridpoints,n_datasets,1);

z_local = NaN([size(xy_local,1) n_datasets]);
for k=1:n_datasets
%     % alternative replace by tri interpolation
%     F = TriScatteredInterp(x_regular,y_regular,z_regular(:,k));
%     z_local_tri(:,k) = F(xy_local);
	z_local(:,k) = griddata(x_regular,y_regular,z_regular(:,k),xy_local(:,1),xy_local(:,2),int_method);
end

if flag_control_fig==1
    figure
    scatter3(xy_local(:,1),xy_local(:,2),z_local(:,1),3,z_local(:,1),'filled');
    hold on
    scatter3(x_regular(:,1),y_regular(:,1),z_regular(:,1),50,z_regular(:,1),'filled');
    view(0,90)
    axis xy
    axis equal
    axis tight
    colorbar
    clear x_regular y_regular z_regular
end


function x = r8to_sl ( n, a, b, job )

%*****************************************************************************80
%
%% R8TO_SL solves a R8TO system.
%
%  Discussion:
%
%    The R8TO storage format is used for a Toeplitz matrix, which is constant
%    along diagonals.  Thus, in an N by N Toeplitz matrix, there are at most
%    2*N-1 distinct entries.  The format stores the N elements of the first
%    row, followed by the N-1 elements of the first column (skipping the
%    entry in the first row).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 March 2004
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real A(2*N-1), the R8TO matrix.
%
%    Input, real B(N) the right hand side vector.
%
%    Input, integer JOB,
%    0 to solve A*X=B,
%    nonzero to solve A'*X=B.
%
%    Output, real X(N), the solution vector.
%
  if ( n < 1 )
    x = [];
    return
  end
%
%  Solve the system with the principal minor of order 1.
%
  r1 = a(1);
  x(1) = b(1) / r1;

  if ( n == 1 )
    return
  end
%
%  Recurrent process for solving the system with the Toeplitz matrix.
%
  for nsub = 2 : n
%
%  Compute multiples of the first and last columns of the inverse of
%  the principal minor of order NSUB.
%
    if ( job == 0 )
      r5 = a(n+nsub-1);
      r6 = a(nsub);
    else
      r5 = a(nsub);
      r6 = a(n+nsub-1);
    end

    if ( 2 < nsub )

      c1(nsub-1) = r2;

      for i = 1 : nsub-2
        if ( job == 0 )
          r5 = r5 + a(n+i) * c1(nsub-i);
          r6 = r6 + a(i+1) * c2(i);
        else
          r5 = r5 + a(i+1) * c1(nsub-i);
          r6 = r6 + a(n+i) * c2(i);
        end
      end

    end

    r2 = -r5 / r1;
    r3 = -r6 / r1;
    r1 = r1 + r5 * r3;

    if ( 2 < nsub )

      r6 = c2(1);
      c2(nsub-1) = 0.0E+00;

      for i = 2 : nsub-1
        r5 = c2(i);
        c2(i) = c1(i) * r3 + r6;
        c1(i) = c1(i) + r6 * r2;
        r6 = r5;
      end

    end

    c2(1) = r3;
%
%  Compute the solution of the system with the principal minor of order NSUB.
%
    if ( job == 0 )
      r5 = a(n+1:n+nsub-1) * x(nsub-1:-1:1)';
    else
      r5 = a(2:nsub) * x(nsub-1:-1:1)';
    end

    r6 = ( b(nsub) - r5 ) / r1;

    x(1:nsub-1) = x(1:nsub-1) + c2(1:nsub-1) * r6;
    x(nsub) = r6;

  end

  return
end


%
% Smooth point-set registration method using neighboring constraints
% -------------------------------------------------------------------
%
% Authors: Gerard Sanromà, René Alquézar and Francesc Serratosa
%
% Contact: gsanorma@gmail.com
% Date: 15/02/2012
%
% Sets arcs among the Eave*n/2 closest pairs of points, where 'n' is the
% number of nodes and 'Eave' is the average number of arcs that we want at
% each node
%
% Input:
%   g: coordinates of the graph nodes
%   Eave: average number of arcs that we want at each node
%
% Output:
%   a1,a2: edges so that i-th edge joins a1(i)-th and a2(i)-th nodes
%

function [a1,a2] = adjZD_eff(g,Eave)

l = length(g);
D = zeros(l);
for it_dim = 1:2
    D = D +(g(:,it_dim)*ones(1,l) - ones(l,1)*g(:,it_dim)').^2;
end
D = sqrt(D);
[v,I] = sort(D(:),'ascend');
[N1,N2] = ind2sub(size(D),I);
indx = find(N1 > N2);
%
[a1,a2] = ind2sub([l,l],I(indx(1:floor(Eave*l/2))));


load('menpo_mtcnn.mat')

% Find the ground truth bboxes
bboxes_gt = bboxes;

bboxes_det = dets;

non_detected = bboxes_det(:,3) == 0;

% Removing the outliers
widths_gt = bboxes_gt(:,3) - bboxes_gt(:,1);
widths_det = bboxes_det(:,3) - bboxes_det(:,1);

heights_gt = bboxes_gt(:,4) - bboxes_gt(:,2);
heights_det = bboxes_det(:,4) - bboxes_det(:,2);

tx_gt =  bboxes_gt(:,1);
ty_gt =  bboxes_gt(:,2);

tx_det = bboxes_det(:,1);
ty_det = bboxes_det(:,2);

bad_det_1 = abs(1 - widths_gt ./ widths_det) > 0.5;
bad_det_2 = abs(1 - heights_gt ./ heights_det) > 0.5;

bad_det_3 = abs((tx_gt - tx_det) ./ widths_det) > 0.4;
bad_det_4 = abs((ty_gt - ty_det) ./ heights_det) > 0.5;

non_detected = non_detected | bad_det_1 | bad_det_2 | bad_det_3 | bad_det_4;

% if the width is quite different from detection then it failed

bboxes_gt = bboxes_gt(~non_detected,:);
bboxes_det = bboxes_det(~non_detected,:);

%% some visualisations
% a = 1;
% plot(gt_labels(a,:,1), gt_labels(a,:,2), '.r');
% hold on;
% bbox = bboxes_detector(a,:);
% % bbox(2) = -bbox(2);
% rectangle('Position', bbox);
% hold off;
% axis equal;

% Want to find out what scaling and translation would lead to the smallest
% RMSE error between initialised landmarks and gt landmarks TODO

% Find the width and height mappings
widths_gt = bboxes_gt(:,3) - bboxes_gt(:,1);
widths_det = bboxes_det(:,3) - bboxes_det(:,1);

heights_gt = bboxes_gt(:,4) - bboxes_gt(:,2);
heights_det = bboxes_det(:,4) - bboxes_det(:,2);

s_width = widths_det \ widths_gt;
s_height = heights_det \ heights_gt;

tx_gt =  bboxes_gt(:,1);
ty_gt =  bboxes_gt(:,2);

tx_det = bboxes_det(:,1);
ty_det = bboxes_det(:,2);

s_tx = median((tx_gt - tx_det) ./ widths_det);
s_ty = median((ty_gt - ty_det) ./ heights_det);

%%
new_widths = widths_det * s_width;
new_heights = heights_det * s_height;
new_tx = widths_det * s_tx + tx_det;
new_ty = heights_det * s_ty + ty_det;

overlaps = zeros(numel(widths_det), 1);
new_overlaps = zeros(numel(widths_det), 1);

for i=1:numel(widths_det)
    bbox_gt = bboxes_gt(i,:);
    bbox_old = bboxes_det(i,:);
    overlaps(i) = overlap(bbox_gt, bbox_old);
    bbox_new = [new_tx(i), new_ty(i), new_tx(i) + new_widths(i), new_ty(i) + new_heights(i)];
    new_overlaps(i) = overlap(bbox_gt, bbox_new);
end

fprintf('Orig - %.3f, now - %.3f\n', mean(overlaps), mean(new_overlaps));


function [ ur, vr, pr ] = resid_stokes1 ( n, x, y )

%*****************************************************************************80
%
%% RESID_STOKES1 returns residuals of the exact Stokes solution #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 January 2015
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Junping Wang, Yanqiu Wang, Xiu Ye,
%    A robust numerical method for Stokes equations based on divergence-free
%    H(div) finite element methods,
%    SIAM Journal on Scientific Computing,
%    Volume 31, Number 4, 2009, pages 2784-2802.
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N), Y(N), the coordinates of the points.
%
%    Output, real UR(N), VR(N), PR(N), the residuals in the U,
%    V and P equations.
%
  ur = zeros ( n, 1 );
  vr = zeros ( n, 1 );
  pr = zeros ( n, 1 );
%
%  Get the right hand sides.
%
  [ f, g, h ] = rhs_stokes1 ( n, x, y );
%
%  Form the functions and derivatives.
%
  for i = 1 : n

    u = - 2.0 ...
          * x(i) ^ 2 * ( x(i) - 1.0 ) ^ 2  ...
          * y(i) * ( y(i) - 1.0 ) * ( 2.0 * y(i) - 1.0 );

    ux = - 2.0  ...
          * ( 4.0 * x(i) ^ 3 - 6.0 * x(i) ^ 2  ...
          + 2.0 * x(i) ) ...
          * y(i) * ( y(i) - 1.0 ) * ( 2.0 * y(i) - 1.0 );

    uxx = - 2.0  ...
          * ( 12.0 * x(i) ^ 2 - 12.0 * x(i) + 2.0 ) ...
          * ( 2.0 * y(i) ^ 3 - 3.0 * y(i) ^ 2 + y(i) );

    uy = - 2.0  ...
          * x(i) ^ 2 * ( x(i) - 1.0 ) ^ 2  ...
          * ( 6.0 * y(i) ^ 2 - 3.0 * y(i) + 1.0 );

    uyy = - 2.0  ...
          * ( x(i) ^ 4 - 2.0 * x(i) ^ 3 + x(i) ^ 2 ) ...
          * ( 12.0 * y(i) - 6.0 );

    v =   2.0  ...
          * x(i) * ( x(i) - 1.0 ) * ( 2.0 * x(i) - 1.0 ) ...
          * y(i) ^ 2 * ( y(i) - 1.0 ) ^ 2;

    vx =   2.0  ...
          * ( 6.0 * x(i) ^ 2 - 6.0 * x(i) + 1.0 ) ...
          * y(i) ^ 2 * ( y(i) - 1.0 ) ^ 2;

    vxx =   2.0  ...
          * ( 12.0 * x(i) - 6.0 ) ...
          * y(i) ^ 2 * ( y(i) - 1.0 ) ^ 2;

    vy =   2.0  ...
          * x(i) * ( x(i) - 1.0 ) * ( 2.0 * x(i) - 1.0 ) ...
          * ( 4.0 * y(i) ^ 3 - 6.0 * y(i) ^ 2  ...
          + 2.0 * y(i) );

    vyy =   2.0  ...
          * x(i) * ( x(i) - 1.0 ) * ( 2.0 * x(i) - 1.0 ) ...
          * ( 12.0 * y(i) ^ 2 - 12.0 * y(i) + 2.0 );

    p = 0.0;
    px = 0.0;
    py = 0.0;

    ur(i) = px - ( uxx + uyy ) - f(i);
    vr(i) = py - ( vxx + vyy ) - g(i);
    pr(i) = ux + vy - h(i);

  end

  return
end


% SOSDEMO5 --- Upper bound for the structured singular value mu
% Section 3.5 of SOSTOOLS User's Manual
%

clear; echo on;
syms x1 x2 x3 x4 x5 x6 x7 x8;
vartable = [x1; x2; x3; x4; x5; x6; x7; x8];

% The matrix under consideration
alpha = 3 + sqrt(3);
beta = sqrt(3) - 1;
a = sqrt(2/alpha);
b = 1/sqrt(alpha);
c = b;
d = -sqrt(beta/alpha);
f = (1 + i)*sqrt(1/(alpha*beta));
U = [a 0; b b; c i*c; d f];
V = [0 a; b -b; c -i*c; -i*f -d];
M = U*V';

% Constructing A(x)'s
gam = 0.8724;

Z = monomials(vartable,1);
for i = 1:4
    H = M(i,:)'*M(i,:) - (gam^2)*sparse(i,i,1,4,4,1);
    H = [real(H) -imag(H); imag(H) real(H)];
    A{i} = (Z.')*H*Z;
end;

% =============================================
% Initialize the sum of squares program
prog = sosprogram(vartable);

% =============================================
% Define SOSP variables

% -- Q(x)'s -- : sums of squares
% Monomial vector: [x1; ... x8]
for i = 1:4
    [prog,Q{i}] = sossosvar(prog,Z);
end;

% -- r's -- : constant sum of squares
Z = monomials(vartable,0);
r = cell(4,4);
for i = 1:4
    for j = (i+1):4
        [prog,r{i,j}] = sossosvar(prog,Z,'wscoeff');
    end;
end;

% =============================================
% Next, define SOSP constraints

% Constraint : -sum(Qi(x)*Ai(x)) - sum(rij*Ai(x)*Aj(x)) + I(x) >= 0
expr = 0;
% Adding term
for i = 1:4
    expr = expr - A{i}*Q{i};
end;
for i = 1:4
    for j = (i+1):4
        expr = expr - A{i}*A{j}*r{i,j};
    end;
end;
% Constant term: I(x) = -(x1^4 + ... + x8^4)
I = -sum(vartable.^4);
expr = expr + I;

prog = sosineq(prog,expr);

% =============================================
% And call solver
prog = sossolve(prog);

% =============================================
% Program is feasible, thus 0.8724 is an upper bound for mu.

echo off


function w_nulldiff = sphNullformer_diff(order, src_dirs)
%SPHNULLFORMER_DIFF Beamweights for a beamformer with nulls at specified directions,
%and an omnidirectional constraint
%
%   For a set of K directions, computes the beamforming weights that places
%   nulls at them while trying to keep the overall response as
%   omnidirectional as possible. Useful when trying to extract
%   reverberant/diffuse sound while suppressing strong directional sources.
%   From a statistical formulation, it is similar to the beamformer
%   proposed in
%
%       Thiergart, O. and Habets, E.A., 2014.
%       Extracting reverberant sound using a linearly constrained minimum variance spatial filter.
%       IEEE Signal Processing Letters, 21(5), pp.630-634.
%
%   but in the SHD instead of the microphone signal domain.
%
%   Inputs:
%       order:  order of SH signals
%       src_dirs:  Kx2 [azi elev] directions to place the nulls
%
%   Outputs:
%       w_nulldiff:  (order+1)^2xK matrix of beamweights, one set per
%           column for each beamforming direction
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% SPHNULLFORMER_DIFF.M - 5/10/2016
% Archontis Politis, archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    nSH = (order+1)^2;
    nSrc = size(src_dirs,1);
    src_dirs2 = [src_dirs(:,1) pi/2-src_dirs(:,2)]; % convert from azi-elev to azi-incl

    % steering vectors in the SHD
    Y_nulldiff = getSH(order, src_dirs2, 'real');

    % compute weight vector
    c = zeros(nSrc+1,1);
    c(1) = 1; % constraint vector
    g = zeros(nSH,1);
    g(1) = 1;
    A = [g Y_nulldiff'];
    w_nulldiff = pinv(A')*c;
end


row=112;
column=92;
NN=row*column;

Class_Train_NUM=5;
Class_Sample_NUM=10; % total
Class_Test_NUM=Class_Sample_NUM-Class_Train_NUM;

Class_NUM=40;
Train_NUM=Class_NUM*Class_Train_NUM; %
Test_NUM=Class_NUM*(Class_Sample_NUM-Class_Train_NUM); %

Eigen_NUM=10;
Disc_NUM=Eigen_NUM;

Train_DAT=zeros(NN,Train_NUM);
s=1;
for r=1:Class_NUM
   for t=1:Class_Train_NUM
     string=['E:\ORL_face\orlnumtotal\s' int2str(r) '_' int2str(t)];
     A=imread(string,'bmp');
     B=im2double(A);
     Train_DAT(:,s)=B(:);
     s=s+1;
   end
end

Test_DAT=zeros(NN,Test_NUM);
s=1;
for r=1:Class_NUM
   for t=Class_Train_NUM+1:Class_Sample_NUM
     string=['E:\ORL_face\orlnumtotal\s' int2str(r) '_' int2str(t)];
     A=imread(string,'bmp');
     B=im2double(A);
     Test_DAT(:,s)=B(:);
     s=s+1;
   end
end

% to center the each training sample and testing sample
% !!! Note that: Centralization have great effection when
%  Cos distance is used, but it has no impact when L2 or L1 distance is used
Mean_Image=mean(Train_DAT,2);
Train_DAT=Train_DAT-Mean_Image*ones(1,Train_NUM);
Test_DAT=Test_DAT-Mean_Image*ones(1,Test_NUM);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[Projection,disc_value]=Eigenface_f(Train_DAT, Disc_NUM);
Eigen_face=zeros(NN,10);

for k=1:1:10
    Eigen_face(:,k)=Projection(:,k);
    Eigen_face(:,k)=mat2gray(Eigen_face(:,k));
end

Eigen_face=reshape(Eigen_face,[row,column,10]);

for count=1:1:10
    subplot(1,10,count);imshow(Eigen_face(:,:,count))
end


function H = compute_infos(I)
level = 256;
dat = double(I);
p = zeros(1, level);
for i = 1 : size(dat, 1)
    for j = 1 : size(dat, 2)
        temp = dat(i,j);
        p(1, temp+1) = p(1, temp+1) + 1;
    end
end
p = p/(size(dat, 1)*size(dat, 2));
H = 0;
for i = 1 : level
    if p(i) ~= 0
        H = H + p(i)*log2(p(i));
    end
end
H = -H;


function homofil(im,d,r,c,n)
%%%%%%%%%%Butterworth high pass filter %%%%%%%%%%%%%%
A=zeros(r,c);
for i=1:r
    for j=1:c
        A(i,j)=(((i-r/2).^2+(j-c/2).^2)).^(.5);
        H(i,j)=1/(1+((d/A(i,j))^(2*n)));
    end
end
%%%%%%%%%%%%%Using it for my application as homomorphic filtering is
%%%%%%%%%%%%%application specific, taking the value of alphaL and alphaH
%%%%%%%%%%%%%values accordingly.
alphaL=.0999;
aplhaH=1.01;
H=((aplhaH-alphaL).*H)+alphaL;
H=1-H;
%%%%%log of image
im_l=log2(1+im);
%%%%%DFT of logged image
im_f=fft2(im_l);
%%%%%Filter Applying DFT image
im_nf=H.*im_f;
%%%%Inverse DFT of filtered image
im_n=abs(ifft2(im_nf));
%%%%%Inverse log
im_e=exp(im_n);
% subplot(1,2,2);
subplot(122)
imshow((im_e),[])

% figure
% imshow(H)


function X = zscore(X)

%ZSCORE standardized z-scores for GRIDobj
%
% Syntax
%
%     Z = zscore(X)
%
% Description
%
%     zscore returns the z-score for each element of GRIDobj X such that
%     all values of X are centered to have mean 0 and scaled to have
%     standard deviation 1.
%
% Input arguments
%
%     X     GRIDobj
%
% Output arguments
%
%     Y     GRIDobj
%
% Example
%
%     DEM = GRIDobj('srtm_bigtujunga30m_utm11.tif');
%     Z = zscore(DEM);
%     imagesc(Z); colorbar
%
% See also: GRIDobj, zscore
%
% Author: Wolfgang Schwanghart (w.schwanghart[at]geo.uni-potsdam.de)
% Date: 15. March, 2016

I = ~isnan(X.Z);
X.Z(I) = (X.Z(I(:)) - mean(X.Z(I(:))))./std(X.Z(I(:)));


% Closed-Form Matting
% This function implements the image matting approach described in
% Anat Levin, Dani Lischinski, Yair Weiss, "A Closed Form Solution to
% Natural Image Matting", IEEE TPAMI, 2008.
% Optional input parameter 'params' can be customized by editing the
% default values in the struct returned by 'getMattingParams('CF').
% - loc_*** define the parameters for the matting Laplacian.

function alpha = closedFormMatting(image, trimap, params, suppressMessages)
    abmtSetup
    tic;
    if ~exist('params', 'var') || isempty(params)
        params = getMattingParams('CF');
    end
    if ~exist('suppressMessages', 'var') || isempty(suppressMessages)
        suppressMessages = false;
    end
    if(~suppressMessages) display('Closed-Form Matting started...'); end

    image = im2double(image);
    trimap = im2double(trimap(:,:,1));

    % Compute matting Laplacian
    unk = trimap < 0.8 & trimap > 0.2;
    dilUnk = imdilate(unk, ones(2 * params.loc_win + 1));
    if(~suppressMessages) display('     Computing matting Laplacian...'); end
    Lap = affinityMatrixToLaplacian(mattingAffinity(image, dilUnk, params.loc_win, params.loc_eps));

    if(~suppressMessages) display('     Solving for alphas...'); end
    alpha = solveForAlphas(Lap, trimap, params.lambda, params.usePCGtoSolve);

    alpha = reshape(alpha, [size(image, 1), size(image, 2)]);

    dur = toc;
    if(~suppressMessages) display(['Done. It took ' num2str(dur) ' seconds.']); end
end


function imResult = blendMode_PinLight(A, B, offsetW, offsetH)
%% Pin Light blending mode: replaces colors depending on the brightness of
%   the blend color. If the blend color is more than 50% brightness and the
%   base color is darker than the blend color, then the base color is
%   replaced with the blend color. If the blend color is less than 50%
%   brightness and the base color is lighter than the blend color, then the
%   base color is replaced with the blend color.
%
% Input:
%       A       -       Base Image
%       B       -       Top Image
%   offsetW     -   move picture B horizontally in respect to the top-left
%                   corner of picture A. Default value = 1.
%   offsetH     -   move picture B vertically in respect to the top-left
%                   corner of picture A. Default value = 1.
%
% Output:
%       imResult    -   Result of the blending, having the same size of the
%                       Base Image A.
%

%% Check Input
a = size(A);
b = size(B);
blendMode_checkInput(nargin, a, b, func2str(@blendMode_PinLight));

if nargin < 3
    offsetW = 1;
    offsetH = 1;
end

if nargin < 4
    offsetH = 1;
end

%% Implementation
imResult = A;

if (((offsetW ~= 1) || (offsetH ~= 1)) || (sum(a == b) ~= length(a)))
    [A, B] = blendMode_ResizeImages(A, B, a, b, offsetW, offsetH);
end

ind = B < 0.5;
indCompl = abs(ind - 1);

tmp1 = blendMode_Darken(A, 2 * B);
tmp2 = blendMode_Lighten(A, 2 * (B - 0.5));
C = ind .* tmp1 + indCompl .* tmp2;

if (((offsetW ~= 1) || (offsetH ~= 1)) || (sum(a == b) ~= length(a)))
    imResult = blendMode_CreateResult(imResult, C, offsetW, offsetH);
else
    imResult = C;
end

end


% StackExchange Signal Processing Q60197
% https://dsp.stackexchange.com/questions/60197
% Implementation of Block Orthogonal Matching Pursuit (BOMP) Algorithm
% References:
%   1.  A
% Remarks:
%   1.  B
% TODO:
% 	1.  C
% Release Notes
% - 1.0.000     19/08/2019
%   *   First release.

%% General Parameters

subStreamNumberDefault = 79;

run('InitScript.m');

figureIdx           = 0;
figureCounterSpec   = '%04d';

generateFigures = ON;

%% Simulation Parameters

numRows         = 8;
% Pay attention: 'numBlocks * numElemntsBlock' should be larger than
% 'numRows'.
numBlocks       = 5;
numElemntsBlock = 3;

paramK = 3;
tolVal = 1e-6;

%% Generate Data

numCols = numBlocks * numElemntsBlock;

mA = randn(numRows, numCols);
vB = randn(numRows, 1);

%% Analysis

% Solving a Problem
vX = SolveLsL0Bomp(mA, vB, numBlocks, paramK, tolVal);

normErr = norm(mA * vX - vB);

disp([' ']);
disp(['Norm of the Error for A x - b - ', num2str(normErr)]);
disp([' ']);

% Comparing to OMP with the number of blocks matches the number of columns
mseErr = mean((SolveLsL0Bomp(mA, vB, numCols, paramK, tolVal) - SolveLsL0Omp(mA, vB, paramK, tolVal)) .^ 2);

disp([' ']);
disp(['MSE of the solution comparison between vanilla OMP to BOMP - ', num2str(mseErr)]);
disp([' ']);

%% Restore Defaults

% set(0, 'DefaultFigureWindowStyle', 'normal');
% set(0, 'DefaultAxesLooseInset', defaultLoosInset);


function combo_test18 ( )

%*****************************************************************************80
%
%% COMBO_TEST18 tests NPART_ENUM, _RSF_LEX_RANK, _RSF_LEX_SUCCESSOR, _RSF_LEX_UNRANK.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2011
%
%  Author:
%
%    John Burkardt
%
  npart = 3;
  n = 12;

  fprintf ( 1, ' \n' );
  fprintf ( 1, 'COMBO_TEST18\n' );
  fprintf ( 1, '  Partitions of N with NPART parts\n' );
  fprintf ( 1, '  in reverse standard form:\n' );
  fprintf ( 1, ' \n' );
  fprintf ( 1, '  NPART_ENUM enumerates,\n' );
  fprintf ( 1, '  NPART_RSF_LEX_RANK ranks,\n' );
  fprintf ( 1, '  NPART_RSF_LEX_SUCCESSOR lists;\n' );
  fprintf ( 1, '  NPART_RSF_LEX_UNRANK unranks.\n' );
%
%  Enumerate.
%
  npartitions = npart_enum ( n, npart );

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  For N = %d\n', n );
  fprintf ( 1, '  and NPART = %d\n', npart );
  fprintf ( 1, '  the number of partitions is %d\n', npartitions );
  fprintf ( 1, ' \n' );
%
%  List.
%
  t = [];
  rank = -1;

  while ( 1 )

    rank_old = rank;

    [ t, rank ] = npart_rsf_lex_successor ( n, npart, t, rank );

    if ( rank <= rank_old )
      break
    end

    fprintf ( 1, '  %4d  ', rank );
    for i = 1 : npart
      fprintf ( 1, '%5d', t(i) );
    end
    fprintf ( 1, '\n' );

  end
%
%  Unrank.
%
  rank = floor ( npartitions / 3 );

  t = npart_rsf_lex_unrank ( rank, n, npart );

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  The element of rank %d:\n', rank );
  fprintf ( 1, ' \n' );
  i4vec_print ( npart, t, '  The element:' );
%
%  Rank.
%
  rank = npart_rsf_lex_rank ( n, npart, t );

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  The rank of the element is computed as %d:\n', rank );

  return
end


function stroud_test46 ( )

%*****************************************************************************80
%
%% TEST46 tests TORUS_5S2, TORUS_6S2, TORUS_14S.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2009
%
%  Author:
%
%    John Burkardt
%
  global FUNC_3D_INDEX;

  r1 = 0.5;
  r2 = 1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST46\n' );
  fprintf ( 1, '  For the interior of a torus,\n' );
  fprintf ( 1, '  TORUS_5S2,\n' );
  fprintf ( 1, '  TORUS_6S2, and\n' );
  fprintf ( 1, '  TORUS_5S2 approximate integrals.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Inner radius = %f\n', r1 );
  fprintf ( 1, '  Outer radius = %f\n', r2 );
  fprintf ( 1, '  Volume = %f\n', torus_volume_3d ( r1, r2 ) );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Rule:        #5S2          #6S2          #14S\n' );
  fprintf ( 1, '    F(X)\n' );
  fprintf ( 1, '\n' );

  num = function_3d_num ( );

  for i = 1 : num

    FUNC_3D_INDEX = i;

    result1 = torus_5s2 ( 'function_3d', r1, r2 );
    result2 = torus_6s2 ( 'function_3d', r1, r2 );
    result3 = torus_14s ( 'function_3d', r1, r2 );

    fname = function_3d_name ( i );

    fprintf ( 1, '  %s  %12f  %12f  %12f\n', fname, result1, result2, result3 );

  end

  return
end


function xList=RKAdaptiveAtTimes(xInit,theTimes,f,initStepSize,order,solutionChoice,RelTol,AbsTol,maxSteps)
%%RKADAPTIVEATTIMES Perform multiple steps of Runge-Kutta propagation
%                   using an adaptive step size. Runge-Kutta methods are
%                   derivative-free techniques for solving ordinary
%                   differential equations. That is, integrating
%                   dx/dt=f(x,t) given initial conditions
%                   (xStart,tSpan(1)). More information on available
%                   algorithms for the steps is given in the comments to
%                   the function RungeKStep.
%
%INPUTS: xInit The initial value of the state (scalar or vector) over which
%              integration is being performed.
%     theTimes The times at which state estimates are desired. theTimes(1)
%              is the time of xInit.
%            f f(xVal,curT) returns the derivative of xVal taken at time
%              curT.
% initStepSize An optional initial step size (in t) to use for the
%              integration. If omitted or an empty matrix is passed, an
%              ad-hoc method is used to find an initial step size.
%        order The order of the Runge-Kutta method. If this parameter is
%              omitted, then the default order of 4 is used. Order can
%              range from 1 to 7.
%  solutionChoice When multiple formulae are implemented, this selects
%              which one to use. Otherwise, this parameter is not used.
%       RelTol The maximum relative error tolerance allowed, a positive
%              scalar. If omitted or an empty matrix is passed, the default
%              value of 1e-3 is used.
%       AbsTol The absolute error tolerance allowed, a positive scalar, of
%              the same for all components of x, or a positive NX1 vector.
%              If omitted or an empty matrix is passed, the default value
%              of 1e-6 is used.
%     maxSteps The maximum allowable number of steps to perform the
%              integration. If omitted, the default of 1024 is used.
%
%OUTPUTS: xList The state at the times given in times xList(:,1) is the
%               same as xInit.
%
%A detailed description of the adaptive step size algorithm can be found in
%the comments of RKAdaptiveOverRange.
%
%May 2015 David Karnick, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<9||isempty(maxSteps))
    maxSteps=1024;
end

if(nargin<8||isempty(AbsTol))
    AbsTol=1e-6;
end

if(nargin<7||isempty(RelTol))
    RelTol=1e-3;
end
if(nargin<6||isempty(solutionChoice))
    solutionChoice=0;
end

if(nargin<5||isempty(order))
    order=5;
end

if(nargin<4||isempty(initStepSize))
    initStepSize=[];
end

xDim=length(xInit);
numTimes=length(theTimes);

xList=zeros(xDim,numTimes);
xList(:,1)=xInit;
for curTime=2:numTimes
    tSpan=theTimes(curTime-1:curTime);
    [xVals,~,~,~,~,initStepSize]=RKAdaptiveOverRange(xList(:,curTime-1),tSpan,f,initStepSize,0,order,solutionChoice,RelTol,AbsTol,maxSteps);
    xList(:,curTime)=xVals(:,end);
end
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 af = naca4gen(iaf)
%
% "naca4gen" Generates the NACA 4 digit airfoil coordinates with desired no.
% of panels (line elements) on it.
%      Author : Divahar Jayaraman (j.divahar@yahoo.com)
%
% INPUTS-------------------------------------------------------------------
%       iaf.designation = NACA 4 digit iaf.designation (eg. '2412') - STRING !
%                 iaf.n = no of panels (line elements) PER SIDE (upper/lower)
% iaf.HalfCosineSpacing = 1 for "half cosine x-spacing"
%                       = 0 to give "uniform x-spacing"
%          iaf.wantFile = 1 for creating airfoil data file (eg. 'naca2412.dat')
%                       = 0 to suppress writing into a file
%       iaf.datFilePath = Path where the data  file has to be created
%                         (eg. 'af_data_folder/naca4digitAF/')
%                         use only forward slash '/' (Just for OS portability)
%
% OUTPUTS------------------------------------------------------------------
% Data:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
%       af.x = x cordinate (nx1 array)
%       af.z = z cordinate (nx1 array)
%      af.xU = x cordinate of upper surface (nx1 array)
%      af.zU = z cordinate of upper surface (nx1 array)
%      af.xL = x cordinate of lower surface (nx1 array)
%      af.zL = z cordinate of lower surface (nx1 array)
%      af.xC = x cordinate of camber line (nx1 array)
%      af.zC = z cordinate of camber line (nx1 array)
%    af.name = Name of the airfoil
%  af.header = Airfoil name ; No of panels ; Type of spacing
%              (eg. 'NACA4412 : [50 panels,Uniform x-spacing]')
%
%
% File:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
% First line : Header eg. 'NACA4412 : [50 panels,Half cosine x-spacing]'
% Subsequent lines : (2*iaf.n+1) rows of x and z values
%
% Typical Inputs:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
% iaf.designation='2312';
% iaf.n=56;
% iaf.HalfCosineSpacing=1;
% iaf.wantFile=1;
% iaf.datFilePath='./'; % Current folder
% iaf.is_finiteTE=0;

% % [[Calculating key parameters-----------------------------------------]]
t=str2num(iaf.designation(3:4))/100;
m=str2num(iaf.designation(1))/100;
p=str2num(iaf.designation(2))/10;

a0= 0.2969;
a1=-0.1260;
a2=-0.3516;
a3= 0.2843;

if iaf.is_finiteTE ==1
    a4=-0.1015; % For finite thick TE
else
    a4=-0.1036;  % For zero thick TE
end

% % [[Giving x-spacing---------------------------------------------------]]
if iaf.HalfCosineSpacing==1
    beta=linspace(0,pi,iaf.n+1)';
    x=(0.5*(1-cos(beta))); % Half cosine based spacing
    iaf.header=['NACA' iaf.designation ' : [' num2str(2*iaf.n) 'panels,Half cosine x-spacing]'];
else
    x=linspace(0,1,iaf.n+1)';
    iaf.header=['NACA' iaf.designation ' : [' num2str(2*iaf.n) 'panels,Uniform x-spacing]'];
end

yt=(t/0.2)*(a0*sqrt(x)+a1*x+a2*x.^2+a3*x.^3+a4*x.^4);

xc1=x(find(x<=p));
xc2=x(find(x>p));
xc=[xc1 ; xc2];

if p==0
    xu=x;
    yu=yt;

    xl=x;
    yl=-yt;

    zc=zeros(size(xc));
else
    yc1=(m/p^2)*(2*p*xc1-xc1.^2);
    yc2=(m/(1-p)^2)*((1-2*p)+2*p*xc2-xc2.^2);
    zc=[yc1 ; yc2];

    dyc1_dx=(m/p^2)*(2*p-2*xc1);
    dyc2_dx=(m/(1-p)^2)*(2*p-2*xc2);
    dyc_dx=[dyc1_dx ; dyc2_dx];
    theta=atan(dyc_dx);

    xu=x-yt.*sin(theta);
    yu=zc+yt.*cos(theta);

    xl=x+yt.*sin(theta);
    yl=zc-yt.*cos(theta);
end
af.name=['NACA ' iaf.designation];

af.x=[flipud(xu) ; xl(2:end)];
af.z=[flipud(yu) ; yl(2:end)];

indx1=1:min( find(af.x==min(af.x)) );  % Upper surface indices
indx2=min( find(af.x==min(af.x)) ):length(af.x); % Lower surface indices
af.xU=af.x(indx1); % Upper Surface x
af.zU=af.z(indx1); % Upper Surface z
af.xL=af.x(indx2); % Lower Surface x
af.zL=af.z(indx2); % Lower Surface z

af.xC=xc;
af.zC=zc;

lecirFactor=0.8;
af.rLE=0.5*(a0*t/0.2)^2;

le_offs=0.5/100;
dyc_dx_le=(m/p^2)*( 2*p-2*le_offs );
theta_le=atan(dyc_dx_le);
af.xLEcenter=af.rLE*cos(theta_le);
af.yLEcenter=af.rLE*sin(theta_le);

% % [[Writing iaf data into file------------------------------------------]]
if iaf.wantFile==1
    F1=iaf.header;
    F2=num2str([af.x af.z]);
    F=strvcat(F1,F2);
    fileName=[iaf.datFilePath 'naca' iaf.designation '.dat'];
    dlmwrite(fileName,F,'delimiter','')
end


%Sigmoid function
function[y] = logistic(x)
y = 1./(1 + exp(-x));


function img_crop = imcrop_pad(img, bbox, padding, sz)
xi = linspace(-1, 1, sz(2));
yi = linspace(-1, 1, sz(1));
[xx,yy] = meshgrid(xi,yi);
yyxx = single([yy(:), xx(:)]') ; % 2xM
[im_h,im_w,im_c,~] = size(img);
if im_c == 1
    img = repmat(img,[1,1,3,1]);
end

target_crop_w = (1+padding)*(bbox(:,3)-bbox(:,1));
target_crop_h = (1+padding)*(bbox(:,4)-bbox(:,2));
target_crop_cx = (bbox(:,1)+bbox(:,3))/2;
target_crop_cy = (bbox(:,2)+bbox(:,4))/2;

cy_t = (target_crop_cy*2/(im_h-1))-1;
cx_t = (target_crop_cx*2/(im_w-1))-1;

h_s = target_crop_h/(im_h-1);
w_s = target_crop_w/(im_w-1);

s = reshape([h_s,w_s]', 2, 1, []); % x,y scaling
t = reshape([cy_t,cx_t]', 2, 1, []); % translation

g = bsxfun(@times, yyxx, s); % scale
g = bsxfun(@plus, g, t); % translate
g = reshape(g, 2, sz(1), sz(2), []);

img_crop = vl_nnbilinearsampler(img, g);

end