% readMNIST by Siddharth Hegde
%
% Description:
% Read digits and labels from raw MNIST data files
% File format as specified on http://yann.lecun.com/exdb/mnist/
% Note: The 4 pixel padding around the digits will be remove
%       Pixel values will be normalised to the [0...1] range
%
% Usage:
% [imgs labels] = readMNIST(imgFile, labelFile, readDigits, offset)
%
% Parameters:
% imgFile = name of the image file
% labelFile = name of the label file
% readDigits = number of digits to be read
% offset = skips the first offset number of digits before reading starts
%
% Returns:
% imgs = 20 x 20 x readDigits sized matrix of digits
% labels = readDigits x 1 matrix containing labels for each digit
%
function [imgs labels] = readMNIST(imgFile, labelFile, readDigits, offset)

    % Read digits
    fid = fopen(imgFile, 'r', 'b');
    header = fread(fid, 1, 'int32');
    if header ~= 2051
        error('Invalid image file header');
    end
    count = fread(fid, 1, 'int32');
    if count < readDigits+offset
        error('Trying to read too many digits');
    end

    h = fread(fid, 1, 'int32');
    w = fread(fid, 1, 'int32');

    if offset > 0
        fseek(fid, w*h*offset, 'cof');
    end

    imgs = zeros([h w readDigits]);

    for i=1:readDigits
        for y=1:h
            imgs(y,:,i) = fread(fid, w, 'uint8');
        end
    end

    fclose(fid);

    % Read digit labels
    fid = fopen(labelFile, 'r', 'b');
    header = fread(fid, 1, 'int32');
    if header ~= 2049
        error('Invalid label file header');
    end
    count = fread(fid, 1, 'int32');
    if count < readDigits+offset
        error('Trying to read too many digits');
    end

    if offset > 0
        fseek(fid, offset, 'cof');
    end

    labels = fread(fid, readDigits, 'uint8');
    fclose(fid);

    % Calc avg digit and count
    imgs = trimDigits(imgs, 4);
    imgs = normalizePixValue(imgs);
    %[avg num stddev] = getDigitStats(imgs, labels);

end

function digits = trimDigits(digitsIn, border)
    dSize = size(digitsIn);
    digits = zeros([dSize(1)-(border*2) dSize(2)-(border*2) dSize(3)]);
    for i=1:dSize(3)
        digits(:,:,i) = digitsIn(border+1:dSize(1)-border, border+1:dSize(2)-border, i);
    end
end

function digits = normalizePixValue(digits)
    digits = double(digits);
    for i=1:size(digits, 3)
        digits(:,:,i) = digits(:,:,i)./255.0;
    end
end


function [x, y, z, varargout] = stlread(filename)
% This function reads an STL file in binary format into matrixes X, Y and
% Z, and C.  C is optional and contains color rgb data in 5 bits.
%
% USAGE: [x, y, z, c] = stlread(filename);
%
% To plot use patch(x,y,z,c), or patch(x,y,z)
%
% Written by Doron Harlev

if nargout>4
    error('Too many output arguments')
end
use_color=(nargout==4);

fid=fopen(filename, 'r'); %Open the file, assumes STL Binary format.
if fid == -1
    error('File could not be opened, check name or path.')
end

ftitle=fread(fid,80,'uchar=>schar'); % Read file title
num_facet=fread(fid,1,'int32'); % Read number of Facets

%fprintf('\nTitle: %s\n', char(ftitle'));
%fprintf('Num Facets: %d\n', num_facet);

% Preallocate memory to save running time
x=zeros(3,num_facet); y=zeros(3,num_facet); z=zeros(3,num_facet);
if use_color
    c=uint8(zeros(3,num_facet));
end

%h = waitbar(0,'Please wait...');
for i=1:num_facet,
    norm=fread(fid,3,'float32'); % normal coordinates, ignored for now
    ver1=fread(fid,3,'float32'); % vertex 1
    ver2=fread(fid,3,'float32'); % vertex 2
    ver3=fread(fid,3,'float32'); % vertex 3
    col=fread(fid,1,'uint16'); % color bytes
    if (bitget(col,16)==1 & use_color)
        r=bitshift(bitand(2^16-1, col),-10);
        g=bitshift(bitand(2^11-1, col),-5);
        b=bitand(2^6-1, col);
        c(:,i)=[r; g; b];
    end
    x(:,i)=[ver1(1); ver2(1); ver3(1)]; % convert to matlab "patch" compatible format
    y(:,i)=[ver1(2); ver2(2); ver3(2)];
    z(:,i)=[ver1(3); ver2(3); ver3(3)];
    if mod(i,floor(num_facet/10))==0
        %waitbar(i/num_facet,h);
    end
end
if use_color
    varargout(1)={c};
end
fclose(fid);
%close(h);

% For more information http://rpdrc.ic.polyu.edu.hk/old_files/stl_binary_format.htm


%BUILDPYRAMID  Constructs the Gaussian pyramid for an image
%
%     dst = cv.buildPyramid(src)
%     dst = cv.buildPyramid(src, 'OptionName',optionValue, ...)
%
% ## Input
% * __src__ Source image. Check cv.pyrDown for the list of supported types.
%
% ## Output
% * __dst__ Destination vector of `Maxlevel+1` images of the same type as
%   `src`. A cell array of images. `dst{1}` will be the same as `src`.
%   `dst{2}` is the next pyramid layer, a smoothed and down-sized `src`, and
%   so on.
%
% ## Options
% * __MaxLevel__ 0-based index of the last (the smallest) pyramid layer. It
%   must be non-negative. default 5
% * __BorderType__ Pixel extrapolation method, ('Constant' isn't supported).
%   See cv.copyMakeBorder for details. Default 'Default'
%
% The function constructs a vector of images and builds the Gaussian pyramid
% by recursively applying cv.pyrDown to the previously built pyramid layers,
% starting from `dst{1}==src`.
%
% See also: cv.pyrDown, cv.pyrUp, vision.Pyramid, impyramid
%


function [ invJ ] = vec2jaclInv( vec )
tolerance = 1e-12;

if size(vec,1) == 3

    phi = vec;

    ph = norm(phi);
    if ph < tolerance
        % If the angle is small, fall back on the series representation
        invJ = vec2jacInvSeries(phi,10);
    else
        axis = phi/norm(phi);
        ph_2 = 0.5*ph;

        invJ =   ph_2 * cot(ph_2)* eye(3)...
               + (1 - ph_2 * cot(ph_2))* axis * axis'...
               - ph_2 * hat(axis);
    end

elseif size(vec,1) == 6

    phi = vec(1:3);
    rho = vec(4:6)

    ph = norm(phi);
    if ph < tolerance
        % If the angle is small, fall back on the series representation
        invJ = vec2jaclInvSeries(phi,10);
    else
        invJsmall = vec2jaclInv( phi );
        Q = vec2Q( vec );
        invJ = [ invJsmall -invJsmall*Q*invJsmall; zeros(3) invJsmall ];
    end
end
end


function [fk,ier]=nufft2d3(nj,xj,yj,cj,iflag,eps,nk,sk,tk)
%NUFFT2D3: Nonuniform FFT in R^2 - Type 3.
%
%  [FK,IER] = NUFFT2D3(NJ,XJ,YJ,CJ,IFLAG,NK,SK,TK);
%
%                 1  nj
%     fk(k)    = -- SUM cj(j) exp(+/-i s(k) xj(j)) exp(+/-i t(k) yj(j))
%                nj j=1
%
%     If (iflag .ge.0) the + sign is used in the exponential.
%     If (iflag .lt.0) the - sign is used in the exponential.
%
%  Input parameters:
%
%     nj     number of sources   (integer)
%     xj,yj  location of sources (real *8)
%
%            on interval [-pi,pi].
%
%     cj     strengths of sources (complex *16)
%     iflag  determines sign of FFT (see above)
%     eps    precision request  (between 1.0e-15 and 1.0e-1)
%     nk     number of (noninteger) Fourier modes computed
%     sk,tk  k-values (locations) of desired Fourier modes
%
%  Output parameters:
%
%     fk     Fourier transform values (complex *16)
%     ier    error return code
%            ier = 0  => normal execution.
%            ier = 1  => precision eps requested is out of range.
%
%

fk=zeros(nk+3,1)+1i*zeros(nk+3,1);
ier=0;
% To avoid error with points aligned, add three points not aligned :
% [0 0], [0 1], [1 0]
sk = [sk;0;0;1];
tk = [tk;0;1;0];
xj = [xj;0;0;1];
yj = [yj;0;1;0];
nj = nj+3;
cj = [cj;0;0;0];
mex_id_ = 'nufft2d3f90(i int[x], i double[], i double[], i dcomplex[], i int[x], i double[x], i int[x], i double[], i double[], io dcomplex[], io int[x])';
[fk, ier] = nufft2d(mex_id_, nj, xj, yj, cj, iflag, eps, nk+3, sk, tk, fk, ier, 1, 1, 1, 1, 1);
fk = fk(1:end-3);


function [E_z, E_zz] = estep_compute_Z_distr(P, S_bar, V, RR, Tr, sigma_sq)
%[E_z, E_zz] = estep_compute_Z_distr(P, S_bar, V, RR, Tr, sigma_sq)

% Computes the distribution over Z given the current parameter estimates (see Eq 17-18)

K = size(V,1)/3;
[T, J] = size(P);
T = T/2;

Pc = P - Tr(:)*ones(1,J);

M_t = zeros(2*J, K);
P_hat_t = zeros(2*J, 1);

E_z = zeros(K, T);
E_zz = zeros(T*K, K);

invSigmaSq_p = eye(2*J)./sigma_sq;
for t=1:T,
   R_t = [RR(t,:); RR(t+T,:)];

   for kk = 1:K,
      M_t(1:J, kk) = (R_t(1,:)*V(1+(kk-1)*3:kk*3, :))';
      M_t(J+1:end, kk) = (R_t(2,:)*V(1+(kk-1)*3:kk*3, :))';
   end
   P_hat_t(1:J) = (R_t(1,:)*S_bar)';
   P_hat_t(J+1:end) = (R_t(2,:)*S_bar)';

   %beta_t = M_t' * inv(M_t*M_t' + sigma_sq*eye(2*J));                      % (Eq 16)
   % Can be computed much more efficiently using the matrix inversion lemma:
   AA = M_t./sigma_sq;
   beta_t = M_t'*(invSigmaSq_p - AA*inv(eye(K) + M_t'*M_t./sigma_sq)*AA');   % (Eq 16)

   E_z(:, t) = beta_t*([Pc(t, :) Pc(t+T, :)]' - P_hat_t);                    % (Eq 17)
   E_zz((t-1)*K+1:t*K,:) = eye(K) - beta_t*M_t + E_z(:,t)*E_z(:,t)';         % (Eq 18)
end


function loglike = gppca_loglikelihood(Y, Qgp, Ytrain)
% loglike = gppca_loglikelihood(Y, Qgp)
%
% Returns posterior predictive loglikelihood.

% Get covariance matrices (needs the training data)
D = cols(Qgp.W);
if isempty(Qgp.CovWp) || isempty(Qgp.CovXp)
  disp('Must run GP PCA to get covariance matrices.');
  Q = vbgppcamv(Ytrain,D,Qgp.inW,Qgp.inX,Qgp.covfuncW,Qgp.logthetaW,Qgp.covfuncX, ...
                Qgp.logthetaX,'init',Qgp, 'maxiter',1, 'updatepseudox', ...
                false, 'updatepseudow', false, 'initpseudow', {Qgp.pseudoW}, ...
                'initpseudox', {Qgp.pseudoX}, 'loglikelihood', false, ...
                'updatehyper', false, 'reconstruct', false);
  Qgp.CovXp = Q.CovXp;
  Qgp.CovWp = Q.CovWp;
end

% Remove empty rows/columns
Obs = ~isnan(Y);
rowrm = (colsum(Obs) == 0);
colrm = (rowsum(Obs) == 0);
if any(rowrm)
  Qgp.inW(:,rowrm) = [];
  Y(rowrm,:) = [];
  Obs(rowrm,:) = [];
end
if any(colrm)
  Qgp.inX(:,colrm) = [];
  Y(:,colrm) = [];
  Obs(:,colrm) = [];
end

% Sample X and W
N = 10; % number of samples
W = zeros([rows(Y), D, N]);
X = zeros([D, cols(Y), N]);
for d=1:cols(Qgp.W)
  W(:,d,:) = reshape(gppredrnd(Qgp.inW, Qgp.pseudoW{d}, Qgp.Wp{d}, ...
                               Qgp.CovWp{d}, Qgp.logthetaW{d}, Qgp.covfuncW{d}, ...
                               N), [rows(W), 1, N]);
  X(d,:,:) = reshape(gppredrnd(Qgp.inX, Qgp.pseudoX{d}, Qgp.Xp{d}, ...
                               Qgp.CovXp{d}, Qgp.logthetaX{d}, Qgp.covfuncX{d}, ...
                               N), [1, cols(X), N]);

  fprintf('Sampled dimension %d/%d\n', d, D);
end

% Sample tau
tau = gamrnd(Qgp.a_tau, 1/Qgp.b_tau, [N,1]);

% Estimate predictive loglikelihood by averaging over samples
loglikes = zeros(N,1);
loglike = 0;
[I,J] = find(Obs);
for k=1:length(I)
  for n=1:N
    mu = W(I(k),:,n)*X(:,J(k),n); % reconstruct
    loglikes(n) = sum(norm_lpdf(Y(I(k),J(k)), mu, sqrt(1/tau(n))));
  end
  loglike = loglike + log( mean(exp(loglikes)) );
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function X = gppredrnd(inputpred, input, postmu, postCov, logtheta, covfunc, N)

I = eye(length(postmu));

if ~iscell(covfunc), covfunc = {covfunc}; end

% Calculate the covariance matrices
Kpp = feval(covfunc{:}, logtheta, input, input);
Kpp = Kpp + 1e-6*I;
Lp = chol(Kpp, 'lower');

Kxp = feval(covfunc{:}, logtheta, inputpred, input);

Kxx = feval(covfunc{:}, logtheta, inputpred, inputpred);

R = solve_tril(Lp, Kxp');
S = solve_triu(Lp', R);

mu = R' * solve_tril(Lp, postmu);
% $$$ szKxx = size(Kxx)
% $$$ szS = size(S)
% $$$ szKpp = size(Kpp)
% $$$ szPostCov = size(postCov)
Cov = Kxx - S' * (Kpp - postCov) * S + 1e-6*eye(length(mu));

X = zeros(length(Kxx), N);
for i=1:N
  X(:,i) = mymvnrnd(mu, Cov);
end


function pde = Maxwellsaddledata
%% MAXWELLDATA2 non-homogenous Dirichlet/Neumann boundary condition
%
%                   curl(mu^(-1)curl u)      = J    in \Omega,
%                                  div u     = 0    in \Omega.
%                                 n \cross u = n � g_D  on \Gamma_D,
%                   n \cross (mu^(-1)curl u) = n � g_N  on \Gamma_N.
%
%   mu = 1;
%   u = [0 cos(x) cos(x)];
%   curlu = [0 sin(x) -sin(x)];
%   J   = u;
%   g_N = n \cross curl u;
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

pde.mu = 1;
pde.J = @J;
pde.exactu = @exactu;
pde.g_D = @exactu;
pde.curlu = @curlu;
pde.g_N = @g_N;

    function s = J(p)
        s = exactu(p);
    end

    function s = exactu(p)
        x = p(:,1); %y = p(:,2); z = p(:,3);
        s = [0*x, cos(x), cos(x)];
    end

    function s = curlu(p)
        x = p(:,1); %y = p(:,2); z = p(:,3);
        s = [0*x, sin(x), -sin(x)];
    end

    function s = g_N(p,n)
        s = curlu(p);
        s = cross(n,s,2);
    end
end


% Digital circuit sizing (vectorized)
% Boyd, Kim, Vandenberghe, and Hassibi, "A Tutorial on Geometric Programming"
% Written for CVX by Almir Mutapcic 02/08/06
% (a figure is generated)
%
% Solves the problem of choosing gate scale factors x_i to give
% minimum ckt delay, subject to limits on the total area and power.
%
%   minimize   D
%       s.t.   P <= Pmax, A <= Amax
%              x >= 1
%
% where variables are scale factors x.
%
% This code uses matrices in order to evaluate signal paths
% through the circuit (thus, it uses vectorize Matlab features).
% It is specific to the digital circuit shown in figure 4 (page 28)
% of GP tutorial paper.

% digital circuit shown in figure 4 (page 28) of GP tutorial paper
m = 7;  % number of cells
n = 8;  % number of edges
A = sparse(m,n);

% A is standard cell-edge incidence matrix of the circuit
% A_ij = 1 if edge j comes out of cell i, -1 if it comes in, 0 otherwise
  A(1,1) =     1;
  A(2,2) =     1;
  A(2,3) =     1;
  A(3,4) =     1;
  A(3,8) =     1;
  A(4,1) =    -1;
  A(4,2) =    -1;
  A(4,5) =     1;
  A(4,6) =     1;
  A(5,3) =    -1;
  A(5,4) =    -1;
  A(5,7) =     1;
  A(6,5) =    -1;
  A(7,6) =    -1;
  A(7,7) =    -1;
  A(7,8) =    -1;

% decompose A into edge outgoing and edge-incoming part
Aout = double(A > 0);
Ain = double(A < 0);

% problem constants
f = [1 0.8 1 0.7 0.7 0.5 0.5]';
e = [1 2 1 1.5 1.5 1 2]';
Cout6 = 10;
Cout7 = 10;

a     = ones(m,1);
alpha = ones(m,1);
beta  = ones(m,1);
gamma = ones(m,1);

% varying parameters for an optimal trade-off curve
N = 20;
Pmax = linspace(10,100,N);
Amax = [25 50 100];
min_delay = zeros(length(Amax),N);

disp('Generating the optimal tradeoff curve...')

for k = 1:length(Amax)
    fprintf( 'Amax = %d:\n', Amax(k) );
    for n = 1:N
        fprintf( '    Pmax = %6.2f: ', Pmax(n) );
        cvx_begin gp quiet
          % optimization variables
          variable x(m)                 % scale factors
          variable t(m)                 % arrival times

          % objective is the upper bound on the overall delay
          % and that is the max of arrival times for output gates 6 and 7
          minimize( max( t(6),t(7) ) )
          subject to
            % input capacitance is an affine function of sizes
            cin = alpha + beta.*x;

            % load capacitance is the input capacitance times the fan-out matrix
            % given by Fout = Aout*Ain'
            cload = (Aout*Ain')*cin;
            cload(6) = Cout6;          % load capacitance of the output gate 6
            cload(7) = Cout7;          % load capacitance of othe utput gate 7

            % delay is the product of its driving resistance R = gamma./x and cload
            d = cload.*gamma./x;

            % power and area definitions
            power = (f.*e)'*x;
            area = a'*x;

            % scale size, power, and area constraints
            x >= 1;
            power <= Pmax(n);
            area <= Amax(k);

            % create timing constraints
            % these constraints enforce t_j + d_j <= t_i over all gates j that drive gate i
            Aout'*t + Ain'*d <= Ain'*t;

            % for gates with inputs not connected to other gates we enforce d_i <= t_i
            d(1:3) <= t(1:3);
        cvx_end
        fprintf( 'delay = %3.2f\n', cvx_optval );
        min_delay(k,n) = cvx_optval;
    end
end

% plot the tradeoff curve
plot(Pmax,min_delay(1,:), Pmax,min_delay(2,:), Pmax,min_delay(3,:));
xlabel('Pmax'); ylabel('Dmin');
disp('Optimal tradeoff curve plotted.')


f = greasy;
a = 128;
M = 1024;
M2 = floor(M/2) + 1;
gl = M;
L = dgtlength(numel(f),a,M);
g = firwin('blackman',gl);
gd = long2fir(gabdual(g,a,M),gl);
N = L/a;
maxit = 100;

corig = dgtreal(f,{'blackman',gl},a,M);
s = abs(corig) + 1i*zeros(size(corig));

cinPtr = libpointer('doublePtr',complex2interleaved(s));
cout = zeros(2*M2,N);
coutPtr = libpointer('doublePtr',cout);

calllib('libphaseret','phaseret_gla_d',cinPtr,libpointer(),g,L,gl,1,a,M,maxit,coutPtr);

cout2 = interleaved2complex(coutPtr.Value);

frec = idgtreal(cout2,{'dual',{'blackman',gl}},a,M);

s2 = dgtreal(frec,{'blackman',gl},a,M);
magnitudeerrdb(s,s2)


function v = mohsst5_explained_variance_fa(W, CovW, X, CovX)

% This is approximate, because this does not take into account the
% correlations between different time instances or spatial locations.

% Remove temporal mean
X = bsxfun(@minus, X, mean(X,2));

% Compute <XX>
XX = X*X';
if ndims(CovX) == 2 && all(size(CovX)==size(X))
  XX = XX + diag(sum(CovX,2));
else
  XX = XX + sum(CovX,3);
end

% Compute weighted <WW>
w = mohsst5_weights();
w = mohsst5_remove_land(w);
WW = W*diag(w)*W';
if ndims(CovW) == 2 && all(size(CovW)==size(W))
  WW = WW + diag(wsum(CovW,w,2));
else
  WW = WW + wsum(CovW,w,3);
end

% Effective number of samples
%N = sum(sum( w(:)*ones(1,size(X,2)) ));
N = sum(w) * size(X,2);

% Explained variance
v = diagprod(WW,XX) / N;


%%
% Test for c-transform in 1D

addpath('../toolbox/');

rep = 'results/c-transform/';
[~,~] = mkdir(rep);

SetAR = @(ar)set(gca, 'PlotBoxAspectRatio', [1 ar 1]);

kx = 10; % #samples
ky = 12;
n = 512/2; % for dense display.

randn('state', 66);
rand('state', 66);

x = rescale( cumsum( rand(kx,1)+.5 ), .05,.95 );
y = rescale( cumsum( rand(ky,1)+.5 ), .05,.95 );
g = linspace(0,1,n);
gx = sort(union(g,x));
gy = sort(union(g,y));

u = rand(kx,1);
u(end) = 1;
u(end/2) = 1;
u(1) = .9;

%%
% Just checking

r = 1;
c = @(x,y)5*abs(x-y).^r;
epsilon = 0;
%
v = ctransform(c, x,u,y, epsilon);
vg = ctransform(c, x,u,gy, epsilon);
%
uu = ctransform(c, y,v,x, epsilon);
uug = ctransform(c, y,v,gx, epsilon);
%
vv = ctransform(c, x,uu,y, epsilon);
vvg = ctransform(c, x,uu,gy, epsilon); % should match vg

rlist = [.5 1 1.5 2];
rlist = [1];

m = .2;

eps_list = linspace(0,.5,7);

for i=1:length(rlist)
    r = rlist(i);
    c = @(x,y)5*abs(x-y).^r;

    clf; hold on;
    for ieps=1:length(eps_list)
        epsilon = eps_list(ieps);
        mc = (ieps-1)/(length(eps_list)-1);
        col = [0 mc 1-mc];
        %
        v = ctransform(c, x,u,y, epsilon);
        vg = ctransform(c, x,u,gy, epsilon);
        %
        plot(gy,vg, '-', 'LineWidth', 2, 'color', col);
    end
    v0 = ctransform(c, x,u,y, 0);
    plot(y,v0, 'b.', 'MarkerSize', 25);
    set(gca, 'XTick', [], 'YTick', []);
    box on; axis tight; axis([0 1 min(v0)-m max(v0)+m]);
    SetAR(1/2);
    saveas(gcf, [rep 'c-transf-v-' num2str(round(10*r)) '.eps'], 'epsc');

    clf; hold on;
    for ieps=1:length(eps_list)
        epsilon = eps_list(ieps);
        mc = (ieps-1)/(length(eps_list)-1);
        col = [1-mc mc 0];
        %
        v = ctransform(c, x,u,y, epsilon);
        vg = ctransform(c, x,u,gy, epsilon);
        %
        uu = ctransform(c, y,v,x, epsilon);
        uug = ctransform(c, y,v,gx, epsilon);
        %
        vv = ctransform(c, x,uu,y, epsilon);
        vvg = ctransform(c, x,uu,gy, epsilon); % should match vg
        %
        plot(gx,uug, '-', 'LineWidth', 2, 'color', col);
    end
    plot(x,u, 'r.', 'MarkerSize', 25);
    set(gca, 'XTick', [], 'YTick', []);
    box on; axis tight; axis([0 1 0-m 1+m]);
    SetAR(1/2);
    saveas(gcf, [rep 'c-transf-u-' num2str(round(10*r)) '.eps'], 'epsc');
end


function [ imat, mat, ifault ] = invmod ( mat, rmod, cmod, nrow, ifault )

%*****************************************************************************80
%
%% INVMOD inverts a matrix using modulo arithmetic.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 May 2013
%
%  Author:
%
%    Original FORTRAN77 version by Roger Payne.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Roger Payne,
%    Inversion of matrices with contents subject to modulo arithmetic,
%    Applied Statistics,
%    Volume 46, Number 2, 1997, pages 295-298.
%
%  Parameters:
%
%    Input/output, integer MAT(NROW*NROW).
%    On input, the matrix to be inverted.
%    On output, the product of the input matrix and IMAT.
%
%    Output, integer IMAT(NROW*NROW), the inverse matrix.
%    If IFAULT = -1 on output, then IMAT is only a left inverse.
%
%    Input, integer RMOD(NROW), the modulus for values in each row.
%
%    Input, integer CMOD(NROW), the modulus for values
%    in each column.
%
%    Input, integer NROW, the order of the matrix.
%
%    Output, integer IFAULT, an error flag.
%    0, no error was detected.
%    -1, only a left inverse could be formed.
%    1, the matrix contains elements that are negative, or too large.
%    2, the matrix contains nonzero elements in mixed modulus positions.
%    3, the matrix cannot be inverted.
%
  imat = [];
%
%  Check that elements in 'mixed-moduli' positions are all zero.
%
  n = 0;
  for i = 1 : nrow
    for j = 1 : nrow

      n = n + 1;

      if ( ( rmod(i) ~= cmod(j) ) && ( 0 < mat(n) ) )
        ifault = 2;
        return
      end

      if ( ( rmod(i) < mat(n) ) || ( mat(n) < 0 ) )
        ifault = 1;
        return
      end

    end
  end

  n = 0;
  for i = 1 : nrow
    for j = 1 : nrow
      n = n + 1;
      imat(n) = 0;
    end
  end
%
%  Sort rows and columns into ascending order of moduli
%
  [ mat, rmod, cmod, rsort, csort ] = msort ( mat, imat, rmod, cmod, nrow );
%
%  Complete initialization of inverse matrix
%
  for n = 1 : nrow + 1 : nrow * nrow
    imat(n) = 1;
  end
%
%  Invert the matrix.
%
  for ir = 1 : nrow

    kir = ( ir - 1 ) * nrow;

    if ( mat(kir+ir) == 0 )
%
%  Find a row JR below IR such that K(JR,IR)>0
%
      all_zero = 1;

      for kjr = kir + nrow + ir : nrow : nrow * nrow
        if ( 0 < mat(kjr) )
          all_zero = 0;
          break;
        end
      end
%
%  Column IR contains all zeros in rows IR or below:
%  look for a row above with zeros to left of column IR
%  and K(JR,IR)>0
%
      if ( all_zero )
        for kjr = ir : nrow : kir
          if ( 0 < mat(kjr) )
            for i = kjr - ir + 1 : kjr - 1
              if ( 0 < mat(i) )
                ifault = 3;
                return
              end
            end
            all_zero = 0;
            break
          end
        end
      end
%
%  Column IR contains all zeros
%
      if ( all_zero )
        continue;
      end
%
%  Switch row JR with row IR
%
      kjr = kjr - ir;

      for i = 1 : nrow

        k = mat(kir+i);
        mat(kir+i) = mat(kjr+i);
        mat(kjr+i) = k;

        k = imat(kir+i);
        imat(kir+i) = imat(kjr+i);
        imat(kjr+i) = k;

      end

    end
%
%  Find a multiplier N such that N*MAT(IR,IR)=1 mod(P{IR})
%
    k = mat(kir+ir);
    for n = 1 : rmod(ir) - 1
      if ( mod ( n * k, rmod(ir) ) == 1 )
        break;
      end
    end
%
%  Multiply row IR by N.
%
    if ( 1 < n )
      for i = kir + 1 : ir * nrow
        mat(i) = mat(i) * n;
        imat(i) = imat(i) * n;
      end
    end
%
%  Subtract MAT(JR,IR) * row IR from each row JR
%
    for kjr = 0 : nrow : nrow * nrow - 1
      n = rmod(ir) - mat(kjr+ir);
      if ( ( kjr ~= kir ) && ( n ~= 0 ) )
        for i = 1 : nrow
          mat(kjr+i)  = mod (  mat(kjr+i) + n *  mat(kir+i), cmod(i) );
          imat(kjr+i) = mod ( imat(kjr+i) + n * imat(kir+i), cmod(i) );
        end
      end
    end

  end
%
%  Check inversion was possible - that result has
%  non-zero elements only on diagonal.
%
  ifault = 0;
%
%  If we encounter a zero diagonal element, then only a left inverse
%  will be formed.
%
  for n = 1 : nrow + 1 : nrow * nrow
    if ( mat(n) == 0 )
      ifault = -1;
    end
    mat(n) = - mat(n);
  end

  for n = 1 : nrow * nrow
    if ( 0 < mat(n) )
      ifault = 3;
      return
    end
  end

  for n = 1 : nrow + 1 : nrow * nrow
    mat(n) = - mat(n);
  end
%
%  Unsort the rows and columns back into their original order.
%
  [ mat, imat, rmod, cmod, rsort, csort ] = musort ( mat, imat, rmod, ...
    cmod, rsort, csort, nrow );

  return
end


function v = norm(F)
%NORM   Frobenius norm of a SPHEREFUNV.
%   V = sqrt(norm(F1).^2 + norm(F2).^2 + norm(F3).^2).

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

% Empty check:
if ( isempty(F) )
    v = [];
    return
end

v = 0;
for jj = 1:3
    v = v + sum(norm(F.components{jj}, 2).^2);
end
v = sqrt(v);

end


function plotChannels(imgIn, imgOut)
%
%
%        plotChannels(imgIn, imgOut)
%
%        This function plots colors channels
%
%     Copyright (C) 2015  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

[r_i, c_i, col_i] = size(imgIn);
[r_o, c_o, col_o] = size(imgOut);

if(col_o ~= col_i)
    error('different images!');
end

hold on;
figure(1);

for i=1:col_i
    tmpX = imgIn(:,:,i);
    tmpX = imresize(tmpX, [16, 16], 'bilinear');
    tmpX = tmpX(:);

    tmpY = imgOut(:,:,i);
    tmpY = imresize(tmpY, [16, 16], 'bilinear');
    tmpY = tmpY(:);

    [tmpX, ind] = sort(tmpX(:), 'ascend');
    windowSize = 16;
    tmpY = tmpY(ind);
    tmpY = filter( (1/windowSize)*ones(1,windowSize), 1, tmpY);

    plot(tmpX, tmpY);
end

end


function [dynpcm2] = mbar2dynpcm2(mbar)
% Convert pressure from millibars to dyne per centimeter squared.
% Chad Greene 2012
dynpcm2 = mbar*1000.00;


function rd = jed_to_rd ( jed )

%*****************************************************************************80
%
%% JED_TO_RD converts a JED to an RD.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Edward Reingold, Nachum Dershowitz,
%    Calendrical Calculations, the Millennium Edition,
%    Cambridge, 2002.
%
%  Parameters:
%
%    Input, real JED, the Julian Ephemeris Date.
%
%    Output, real RD, the RD date.
%
  rd_epoch = epoch_to_jed_rd ( );

  rd = jed - rd_epoch;

  return
end


function movingcircle
% MOVINGCIRCLE will track the interface defined by x^2+y^2 = (0.5-t)^2
%--------------------------------------------------------------------------
% Copyright (C) 2008 Long Chen. See COPYRIGHT.txt for details.
%--------------------------------------------------------------------------

close all; clear all;
%---------------------- Parameters ----------------------------------------
figure(1); set(gcf,'Units','normal'); set(gcf,'Position',[0,0,0.8,0.4]);
t = 0; dt = 0.075; maxIt = 30;
%---------------------- Initial Grid --------------------------------------
node = [-1 -1; 1 -1; 1 1; -1 1];
elem = [2 3 1; 4 1 3];
N0 = size(elem,1);
for i = 1:5
	[node,elem] = uniformbisect(node,elem);
end
u = -sign(f(node,t));
subplot(1,2,1); showmesh(node,elem); pause(0.1)
subplot(1,2,2);  showsolution(node,elem,u,[0,90]); colorbar;
for k = 1:maxIt
	if (mod(k,4) == 0), t = t + dt; end
	%---------- detect element cross interface or away from interface -----
	eta = abs(sign(f(node(elem(:,1),:),t)) + sign(f(node(elem(:,2),:),t))...
            + sign(f(node(elem(:,3),:),t)));
	refineElem = find(eta < 3);
    coarsenElem = find(eta == 3);
    %---------- refine elements cross the interface -----------------------
    [node,elem] = bisect(node,elem,refineElem);
    u = -sign(f(node,t));
    subplot(1,2,1); showmesh(node,elem); pause(0.025)
    subplot(1,2,2);  showsolution(node,elem,u); view(2); colorbar;
    %---------- coarsen elements away from the interface ------------------
    [node,elem] = coarsen(node,elem,coarsenElem);
    u = -sign(f(node,t));
    subplot(1,2,1);  showmesh(node,elem); pause(0.025)
    subplot(1,2,2);  showsolution(node,elem,u); view(2); colorbar;
end
end
%-------------------- End of MOVINGCIRCLE --------------------------------

%-------------------- Sub functions called by MOVINGCIRCLE ---------------
function z = f(p,t)
z = sum(p.^2,2) - (0.5-t)^2;
end


function x_new = sls1mm(f, x_0, opt, gradf, varargin)
%SLS1MM  Markov Chain Monte Carlo sampling using Slice Sampling
%
%  Description
%    SLS1MM is faster streamlined version of SLS for generating samples
%    from one dimensional distribution with known min-max limits.
%
%    SAMPLE = SLS1MM(F, X, OPTIONS) uses slice sampling to generate
%      single value from the *one dimensional* distribution P ~ EXP(-F),
%      where F is the first argument to SLS1MM. Markov chain starts from
%      point X and the sampling is made using min-max slice sampling.
%      See SLS1MM_OPT for details.
%
%    SAMPLES = SLS1MM(F, X, OPTIONS, [], P1, P2, ...) allows additional
%      arguments to be passed to F(). The fourth argument is ignored,
%      but included for compatibility with HMC2 and the optimisers.
%
%  See SLS1MM_OPT for the optional parameters in the OPTIONS structure.
%  Note, that unlike in SLS, missing fields give an error.
%
%  See also
%    SLS1MM_OPT, SLS

%  Based on "Slice Sampling" by Radford M. Neal in "The Annals of Statistics"
%  2003, Vol. 31, No. 3, 705-767, (c) Institute of Mathematical Statistics, 2003

%       Copyright (c) 2003-2004 Toni Auranen
%       Copyright (c) 2004 Aki Vehtari

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

% Set up some variables
maxiter = opt.maxiter;
l = opt.mmlimits(1);
r = opt.mmlimits(2);

% Generate sample
y = -f(x_0,varargin{:}) + log(rand);
x_new = x_0;
for iter=1:maxiter
  x_new = l + (r-l).*rand;
  y_new = -f(x_new,varargin{:});
  if y < y_new
    return;
  end
  if x_new < x_0
    l = x_new;
  else
    r = x_new;
  end
end
if iter+1 > maxiter
  fprintf('Maximum number (%d) of iterations reached during shrinkage.\n',maxiter);
  error('Check function F, decrease the interval ''mmlimits'' or increase the value of ''maxiter''.');
end


%% This file is the ODE file used to simulate the Michaelis-Menten kinetics.
% Date: 04/24/2019
% Coded By: K.Kahirman

% Here Vmax is rhe maximum rate of reaction time
% Km is the concentration of half-maximal reaction rate
%

%% ODE functions
function dx=MMK_ODE(t,x,jx,Vmax,Km)
dx=jx-(Vmax*x(1,:)./(Km+x(1,:)));


function [ uv ] = coords2uv( coords, width, height )
%COORDS2UV Image coordinates (xy) to uv
%   Detailed explanation goes here
middleX = width/2+0.5;
middleY = height/2+0.5;
uv = [(coords(:,1)-middleX)./width*2*pi -(coords(:,2)-middleY)./height*pi];

end


function [Y] = diagonalize(X,samplesize)
% ----------------------- Input ------------------------------
% X: samples of all tasks (each row is a sample)
% samplesize: the i-th entry is the sample size of the i-th task
% ----------------------- Output -----------------------------
% Y: sparse data matrix which is diagonal

tasknum = length(samplesize); % the number of tasks
[totalnum,dim] = size(X);
row = zeros(dim*totalnum,1); col = row; datavec = row;
accumsize = 0; accumind = 0;
for i = 1:tasknum
    accumsize = accumsize + samplesize(i);
    accumind = accumind + dim*samplesize(i);
    indsample = (accumsize-samplesize(i)+1 : accumsize)';
    indnz = (accumind-dim*samplesize(i)+1 : accumind)';
    row(indnz) = repmat(indsample,dim,1);
    col(indnz) = reshape(repmat(dim*(i-1)+1:dim*i,samplesize(i),1),dim*samplesize(i),1);
%     Xi = X(indsample,:)/sqrt(tasknum*samplesize(i));
    Xi = X(indsample,:);
    datavec(indnz) = Xi(:);
end
Y = sparse(row,col,datavec,totalnum,dim*tasknum);


% Test file for singfun constructor.

function pass = test_singfun_constructor(pref)

% Get preferences:
if ( nargin < 1 )
    pref = chebfunpref();
end

%%
% Select some random points as sample points
% These random points are in [-1, 1]
seedRNG(7890)
x = -1 + 2*rand(1, 100);
x = sort(x);

% Some arbitrary values to use for exponents.
a = 0.338745372057174;
b = 0.561224728136042;
a_int = 2;
b_int = 5;

%% Test calling syntax when the user provides exponents

% Negative fractional exponents
fh = @(x) sin(x)./((1+x).^a.*(1-x).^b);
data = struct();
data.exponents = [-a, -b];
f = singfun(fh, data, pref);
data.exponents = [-a, -b];
data.singType = {'sing', 'sing'};
g = singfun(fh, data, pref);
pass(1) = isequal(f,g);
pass(2) = ~any(f.exponents + [a,b]);
pass(3) = ~any(g.exponents + [a,b]);
pass(4) = norm(feval(fh,x) - feval(f,x), inf) < 1e2*eps;

%%
% Positive fractional exponents
fh = @(x) sin(x).*(1+x).^a.*(1-x).^b;
data = struct();
data.exponents = [a, b];
f = singfun(fh, data, pref);
data.exponents = [a, b];
data.singType = {'root', 'root'};
g = singfun(fh, data, pref);
pass(5) = isequal(f,g);
pass(6) = ~any(f.exponents - [a,b]);
pass(7) = ~any(g.exponents - [a,b]);
pass(8) = norm(feval(fh,x) - feval(f,x), inf) < 1e1*eps;

%%
% Negative integer exponents
fh = @(x) exp(x)./((1+x).^a_int.*(1-x).^b_int);
data = struct();
data.exponents = [-a_int, -b_int];
f = singfun(fh, data, pref);
data.exponents = [-a_int, -b_int];
data.singType = {'pole', 'pole'};
g = singfun(fh, data, pref);
pass(9) = isequal(f,g);
pass(10) = ~any(f.exponents + [a_int, b_int]);
pass(11) = ~any(g.exponents + [a_int, b_int]);
% don't check near end-points
xx = x(20:80);
pass(12) = norm(feval(fh,xx) - feval(f,xx), inf) < 1e2*eps;

%% Test Syntax and construction when the user doesn't provide exponents
%
% Negative fractional exponents
fh = @(x) exp(sin(x))./((1+x).^a.*(1-x).^b);
f = singfun(fh);
pass(13) = norm(f.exponents + [a,b], inf) < pref.blowupPrefs.exponentTol;
pass(14) = norm(feval(fh,x) - feval(f,x), inf) < 1e5*eps;

%%
% Positive fractional exponents
fh = @(x) sin(exp(cos(x))).*(1+x).^a.*(1-x).^b;
f = singfun(fh);
pass(15) = norm(f.exponents - [a,b], inf) < pref.blowupPrefs.exponentTol;
pass(16) = norm(feval(fh,x) - feval(f,x), inf) < 1e4*eps;

%%
% Negative integer exponents
fh = @(x) exp(sin(x.^2))./((1+x).^a_int.*(1-x).^b_int);
f = singfun(fh);
pass(17) = norm(f.exponents + [a_int, b_int], inf) < pref.blowupPrefs.exponentTol;
xx = x(20:80);
pass(18) = norm(feval(fh,xx) - feval(f,xx), inf) < 1e3*eps;

%%
% Construction with smoothfuns:
f = smoothfun.constructor( @(x) sin(x));
s = singfun(f);
pass(19) = iszero(f - s.smoothPart);
data = struct();
data.exponents = [-1.5, -1];
data.singType = {'sing', 'sing'};
s = singfun(f, data, pref);
pass(20) = iszero(f - s.smoothPart);
pass(21) = norm(s.exponents - [-1.5, -1], inf) < pref.blowupPrefs.exponentTol;

%%
% Construction from double:
f = singfun(42);
pass(22) = iszero(f - 42);
data = struct();
data.exponents = [1.5, 1];
data.singType = {'sing', 'sing'};
f = singfun(42, data, pref);
g = singfun(@(x) 42+0*x);
g.exponents = [1.5, 1];
pass(23) = iszero(f - g);
pass(24) = norm(s.exponents - [-1.5, -1], inf) < pref.blowupPrefs.exponentTol;

end


% SUMMARY:  Log Forward Backward, to solve overflow problem
% AUTHOR:   QIUQIANG KONG
% Created:  18-11-2015
% Modified: 19-11-2015 modify max to sum (exact solution)
%           20-11-2015 add a ceiling for logbeta
%           21-11-2015 use log instead of all to avoid overfit
%           25-11-2015 Add annotation
% -----------------------------------------------------------
% input:
%   p_xn_given_zn  p(xn|zn), size: N*Q
%   p_start        p(z1), size: Q
%   A              p(zn|zn-1), size: Q*Q
% output:
%   loggamma       ln p(zn|X), size: N*Q
%   logksi         ln p(zn,zn-1|X), size: N*Q*Q
%   loglik         ln p(X), to monitor convergence
% ===========================================================
function [loggamma, logksi, loglik] = LogForwardBackward(logp_xn_given_zn, p_start, A)
    % reserve space
    [N,Q] = size(logp_xn_given_zn);
    logalpha = zeros(N,Q);
    logbeta = zeros(N,Q);
    logc = zeros(N,1);
    loggamma = zeros(N,Q);
    logksi = zeros(N,Q,Q);

    % init log alpha(z1), log beta(zN), c(1)
    tmp = logp_xn_given_zn(1,:) + log(p_start);
    logc(1) = log( sum( exp( tmp - max(tmp) ) ) ) + max(tmp);
    logalpha(1,:) = -logc(1) + logp_xn_given_zn(1,:) + log(p_start);
    logbeta(N,:) = 0;

    % calculate logalpha, c
    for n = 2:N
        tmp = bsxfun(@plus, bsxfun(@plus, log(A), logalpha(n-1,:)'), logp_xn_given_zn(n,:));
        logc(n) = log ( sum( sum ( exp ( tmp - max(tmp(:)) ) ) ) ) + max(tmp(:));
        for q = 1:Q
            tmp2 = logalpha(n-1,:) + log(A(:,q)');
            if (isinf(max(tmp2)))
                logalpha(n,q) = -inf;
            else
                logalpha(n,q) = -logc(n) + logp_xn_given_zn(n,q) + log( sum( exp( tmp2 - max(tmp2) ) ) ) + max(tmp2);
            end
        end
    end

    % calculate logbeta
    for n = N-1:-1:1
        for q = 1:Q
            tmp = logbeta(n+1,:) + logp_xn_given_zn(n+1,:) + log(A(q,:));
            logbeta(n,q) = -logc(n+1) + log( sum( exp( tmp - max(tmp) ) ) ) + max(tmp);
        end
    end

    % calculate loggamma
    loggamma = logalpha + logbeta;

    % calculate logksi
    for n = 2:N
        logksi(n,:,:) = -logc(n) + bsxfun(@plus, bsxfun(@plus, log(A), logalpha(n-1,:)'), logp_xn_given_zn(n,:) + logbeta(n,:));
    end
    logksi(1,:,:) = [];

    % calculate likelihood
    loglik = sum(logc);
end


function [C,PI,PC] = dual(V,F)
  % DUAL Construct the dual polygonal mesh of a given triangle mesh
  %
  % [C,PI,PC] = dual(V,F)
  %
  % Inputs:
  %   V  #V by dim list of vertex positions
  %   F  #F by 3 list of indices into rows of V
  % Outputs:
  %   C  #C by dim list dual vertex positions
  %   PI  #PI stream of polygon indices into rows of C
  %   PC  #V+1 list of cumulative sum of dual face valences
  %
  % See also: polygons_to_triangles, dual_subdivide
  %
  assert(~any(on_boundary(F),'all'));
  [~,C] = circumradius(V,F);

  % vertex indices
  I = F(:);
  % only keep unique
  [I,J] = unique(I);
  % index in faces
  IF = mod(J-1,size(F,1))+1;
  % order in face
  IC = floor((J-1)/size(F,1))+1;

  [Fp,Fi] = triangle_triangle_adjacency(F);

  p1 = @(I) mod(I,3)+1;

  %NF = Fp(sub2ind(size(Fp),IF,p1(IC)));
  %NC = p1(Fi(sub2ind(size(Fi),IF,p1(IC))));
  %clf;
  %hold on;
  %tsurf(F,V,'FaceColor','w',falpha(0.8,1));
  %qvr(V(I,:),C(IF,:)-V(I,:),0,'LineWidth',2);
  %qvr(C(IF,:),C(NF,:)-C(IF,:),0,'LineWidth',2);
  %qvr(C(NF,:),V(F(sub2ind(size(F),NF,NC)),:)-C(NF,:),0,'LineWidth',2);
  %hold off;
  %axis equal;
  %view(3);

  % starting face
  IF0 = IF;
  % ledger of vertex-face pairs in order of observation
  L = [];
  while true
    L = [L;I IF];
    N = sub2ind(size(Fp),IF,p1(IC));
    NF = Fp(N);
    NC = p1(Fi(N));
    %clf;
    %hold on;
    %tsurf(F,V,'FaceColor','w',falpha(0.8,1));
    %qvr(V(I,:),C(IF,:)-V(I,:),0,'LineWidth',2);
    %qvr(C(IF,:),C(NF,:)-C(IF,:),0,'LineWidth',2);
    %qvr(C(NF,:),V(F(sub2ind(size(F),NF,NC)),:)-C(NF,:),0,'LineWidth',2);
    %hold off;
    %axis equal;
    %view(3);
    %pause
    IF = NF;
    IC = NC;

    keep = find(IF ~= IF0);
    IF = IF(keep);
    IC = IC(keep);
    I = I(keep);
    IF0 = IF0(keep);
    if isempty(keep)
      break;
    end
  end

  % stable sort by vertex
  [~,S] = sort(L(:,1));
  L = L(S,:);

  PC = cumsum([0;accumarray(L(:,1),1)]);
  PI = L(:,2);
end


function [Vseq,Dseq] = eigenshuffle(Asequence)
% eigenshuffle: Consistent sorting for an eigenvalue/vector sequence
% [Vseq,Dseq] = eigenshuffle(Asequence)
%
% Includes munkres.m (by gracious permission from Yi Cao)
% to choose the appropriate permutation. This greatly
% enhances the speed of eigenshuffle over my previous
% release.
%
% http://www.mathworks.com/matlabcentral/fileexchange/20652
%
% Arguments: (input)
%  Asequence - an array of eigenvalue problems. If
%      Asequence is a 3-d numeric array, then each
%      plane of Asequence must contain a square
%      matrix that will be used to call eig.
%
%      Eig will be called on each of these matrices
%      to produce a series of eigenvalues/vectors,
%      one such set for each eigenvalue problem.
%
% Arguments: (Output)
%  Vseq - a 3-d array (pxpxn) of eigenvectors. Each
%      plane of the array will be sorted into a
%      consistent order with the other eigenvalue
%      problems. The ordering chosen will be one
%      that maximizes the energy of the consecutive
%      eigensystems relative to each other.
%
%  Dseq - pxn array of eigen values, sorted in order
%      to be consistent with each other and with the
%      eigenvectors in Vseq.
%
% Example:
%  Efun = @(t) [1 2*t+1 t^2 t^3;2*t+1 2-t t^2 1-t^3; ...
%               t^2 t^2 3-2*t t^2;t^3 1-t^3 t^2 4-3*t];
%
%  Aseq = zeros(4,4,21);
%  for i = 1:21
%    Aseq(:,:,i) = Efun((i-11)/10);
%  end
%  [Vseq,Dseq] = eigenshuffle(Aseq);
%
% To see that eigenshuffle has done its work correctly,
% look at the eigenvalues in sequence, after the shuffle.
%
% t = (-1:.1:1)';
% [t,Dseq']
% ans =
%        -1     8.4535           5      2.3447     0.20181
%      -0.9     7.8121      4.7687      2.3728     0.44644
%      -0.8     7.2481        4.56      2.3413     0.65054
%      -0.7     6.7524      4.3648      2.2709      0.8118
%      -0.6     6.3156      4.1751      2.1857     0.92364
%      -0.5     5.9283      3.9855      2.1118     0.97445
%      -0.4     5.5816      3.7931      2.0727     0.95254
%      -0.3     5.2676      3.5976      2.0768       0.858
%      -0.2     4.9791      3.3995      2.1156     0.70581
%      -0.1     4.7109         3.2      2.1742     0.51494
%         0     4.4605           3      2.2391     0.30037
%       0.1     4.2302         2.8      2.2971    0.072689
%       0.2     4.0303      2.5997      2.3303    -0.16034
%       0.3     3.8817      2.4047      2.3064    -0.39272
%       0.4     3.8108      2.1464      2.2628    -0.62001
%       0.5     3.8302      1.8986      2.1111    -0.83992
%       0.6     3.9301      1.5937      1.9298     -1.0537
%       0.7     4.0927      1.2308       1.745     -1.2685
%       0.8     4.3042     0.82515      1.5729     -1.5023
%       0.9     4.5572     0.40389      1.4272     -1.7883
%         1     4.8482  -8.0012e-16     1.3273     -2.1755
%
% Here, the columns are the shuffled eigenvalues.
% See that the second eigenvalue goes to zero, but
% the third eigenvalue remains positive. We can plot
% eigenvalues and see that they have crossed, near
% t = 0.35 in Efun.
%
% plot(-1:.1:1,Dseq')
%
% For a better appreciation of what eigenshuffle did,
% compare the result of eig directly on Efun(.3) and
% Efun(.4). Thus:
%
% [V3,D3] = eig(Efun(.3))
% V3 =
%     -0.74139      0.53464     -0.23551       0.3302
%      0.64781       0.4706     -0.16256      0.57659
%    0.0086542     -0.44236     -0.89119      0.10006
%     -0.17496     -0.54498      0.35197      0.74061
%
% D3 =
%     -0.39272            0            0            0
%            0       2.3064            0            0
%            0            0       2.4047            0
%            0            0            0       3.8817
%
% [V4,D4] = eig(Efun(.4))
% V4 =
%     -0.73026      0.19752      0.49743      0.42459
%      0.66202      0.21373      0.35297      0.62567
%     0.013412     -0.95225      0.25513      0.16717
%     -0.16815    -0.092308     -0.75026      0.63271
%
% D4 =
%     -0.62001            0            0            0
%            0       2.1464            0            0
%            0            0       2.2628            0
%            0            0            0       3.8108
%
% With no sort or shuffle applied, look at V3(:,3). See
% that it is really closest to V4(:,2), but with a sign
% flip. Since the signs on the eigenvectors are arbitrary,
% the sign is changed, and the most consistent sequence
% will be chosen. By way of comparison, see how the
% eigenvectors in Vseq have been shuffled, the signs
% swapped appropriately.
%
% Vseq(:,:,14)
% ans =
%       0.3302      0.23551     -0.53464      0.74139
%      0.57659      0.16256      -0.4706     -0.64781
%      0.10006      0.89119      0.44236   -0.0086542
%      0.74061     -0.35197      0.54498      0.17496
%
% Vseq(:,:,15)
% ans =
%      0.42459     -0.19752     -0.49743      0.73026
%      0.62567     -0.21373     -0.35297     -0.66202
%      0.16717      0.95225     -0.25513    -0.013412
%      0.63271     0.092308      0.75026      0.16815
%
% See also: eig
%
% Author: John D'Errico
% e-mail: woodchips@rochester.rr.com
% Release: 3.0
% Release date: 2/18/09

% Is Asequence a 3-d array?
Asize = size(Asequence);
if (Asize(1)~=Asize(2))
  error('Asequence must be a (pxpxn) array of eigen-problems, each of size pxp')
end
p = Asize(1);
if length(Asize)<3
  n = 1;
else
  n = Asize(3);
end

% the initial eigenvalues/vectors in nominal order
Vseq = zeros(p,p,n);
Dseq = zeros(p,n);
for i = 1:n
  [V,D] = eig(Asequence(:,:,i));
  D = diag(D);
  % initial ordering is purely in decreasing order.
  % If any are complex, the sort is in terms of the
  % real part.
  [junk,tags] = sort(real(D),1,'descend');

  Dseq(:,i) = D(tags);
  Vseq(:,:,i) = V(:,tags);
end

% was there only one eigenvalue problem?
if n < 2
  % we can quit now, having sorted the eigenvalues
  % as best as we could.
  return
end

% now, treat each eigenproblem in sequence (after
% the first one.)
for i = 2:n
  % compute distance between systems
  V1 = Vseq(:,:,i-1);
  V2 = Vseq(:,:,i);
  D1 = Dseq(:,i-1);
  D2 = Dseq(:,i);
  dist = (1-abs(V1'*V2)).*sqrt( ...
    distancematrix(real(D1),real(D2)).^2+ ...
    distancematrix(imag(D1),imag(D2)).^2);

  % Is there a best permutation? use munkres.
  % much faster than my own mintrace, munkres
  % is used by gracious permission from Yi Cao.
  reorder = munkres(dist);

  Vseq(:,:,i) = Vseq(:,reorder,i);
  Dseq(:,i) = Dseq(reorder,i);

  % also ensure the signs of each eigenvector pair
  % were consistent if possible
  S = squeeze(real(sum(Vseq(:,:,i-1).*Vseq(:,:,i),1))) < 0;
  Vseq(:,S,i) = -Vseq(:,S,i);
end

% =================
% end mainline
% =================
% begin subfunctions
% =================

function d = distancematrix(vec1,vec2)
% simple interpoint distance matrix
[vec1,vec2] = ndgrid(vec1,vec2);
d = abs(vec1 - vec2);

function [assignment,cost] = munkres(costMat)
% MUNKRES   Munkres (Hungarian) Algorithm for Linear Assignment Problem.
%
% [ASSIGN,COST] = munkres(COSTMAT) returns the optimal column indices,
% ASSIGN assigned to each row and the minimum COST based on the assignment
% problem represented by the COSTMAT, where the (i,j)th element represents the cost to assign the jth
% job to the ith worker.
%

% This is vectorized implementation of the algorithm. It is the fastest
% among all Matlab implementations of the algorithm.

% Examples
% Example 1: a 5 x 5 example
%{
[assignment,cost] = munkres(magic(5));
disp(assignment); % 3 2 1 5 4
disp(cost); %15
%}
% Example 2: 400 x 400 random data
%{
n=400;
A=rand(n);
tic
[a,b]=munkres(A);
toc                 % about 2 seconds
%}
% Example 3: rectangular assignment with inf costs
%{
A=rand(10,7);
A(A>0.7)=Inf;
[a,b]=munkres(A);
%}
% Reference:
% "Munkres' Assignment Algorithm, Modified for Rectangular Matrices",
% http://csclab.murraystate.edu/bob.pilgrim/445/munkres.html

% version 2.0 by Yi Cao at Cranfield University on 10th July 2008

assignment = zeros(1,size(costMat,1));
cost = 0;

costMat(costMat~=costMat)=Inf;
validMat = costMat<Inf;
validCol = any(validMat,1);
validRow = any(validMat,2);

nRows = sum(validRow);
nCols = sum(validCol);
n = max(nRows,nCols);
if ~n
    return
end

maxv=10*max(costMat(validMat));

dMat = zeros(n) + maxv;
dMat(1:nRows,1:nCols) = costMat(validRow,validCol);

%*************************************************
% Munkres' Assignment Algorithm starts here
%*************************************************

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   STEP 1: Subtract the row minimum from each row.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
minR = min(dMat,[],2);
minC = min(bsxfun(@minus, dMat, minR));

%**************************************************************************
%   STEP 2: Find a zero of dMat. If there are no starred zeros in its
%           column or row start the zero. Repeat for each zero
%**************************************************************************
zP = dMat == bsxfun(@plus, minC, minR);

starZ = zeros(n,1);
while any(zP(:))
    [r,c]=find(zP,1);
    starZ(r)=c;
    zP(r,:)=false;
    zP(:,c)=false;
end

while 1
%**************************************************************************
%   STEP 3: Cover each column with a starred zero. If all the columns are
%           covered then the matching is maximum
%**************************************************************************
    if all(starZ>0)
        break
    end
    coverColumn = false(1,n);
    coverColumn(starZ(starZ>0))=true;
    coverRow = false(n,1);
    primeZ = zeros(n,1);
    [rIdx, cIdx] = find(dMat(~coverRow,~coverColumn)==bsxfun(@plus,minR(~coverRow),minC(~coverColumn)));
    while 1
        %**************************************************************************
        %   STEP 4: Find a noncovered zero and prime it.  If there is no starred
        %           zero in the row containing this primed zero, Go to Step 5.
        %           Otherwise, cover this row and uncover the column containing
        %           the starred zero. Continue in this manner until there are no
        %           uncovered zeros left. Save the smallest uncovered value and
        %           Go to Step 6.
        %**************************************************************************
        cR = find(~coverRow);
        cC = find(~coverColumn);
        rIdx = cR(rIdx);
        cIdx = cC(cIdx);
        Step = 6;
        while ~isempty(cIdx)
            uZr = rIdx(1);
            uZc = cIdx(1);
            primeZ(uZr) = uZc;
            stz = starZ(uZr);
            if ~stz
                Step = 5;
                break;
            end
            coverRow(uZr) = true;
            coverColumn(stz) = false;
            z = rIdx==uZr;
            rIdx(z) = [];
            cIdx(z) = [];
            cR = find(~coverRow);
            z = dMat(~coverRow,stz) == minR(~coverRow) + minC(stz);
            rIdx = [rIdx(:);cR(z)];
            cIdx = [cIdx(:);stz(ones(sum(z),1))];
        end
        if Step == 6
            % *************************************************************************
            % STEP 6: Add the minimum uncovered value to every element of each covered
            %         row, and subtract it from every element of each uncovered column.
            %         Return to Step 4 without altering any stars, primes, or covered lines.
            %**************************************************************************
            [minval,rIdx,cIdx]=outerplus(dMat(~coverRow,~coverColumn),minR(~coverRow),minC(~coverColumn));
            minC(~coverColumn) = minC(~coverColumn) + minval;
            minR(coverRow) = minR(coverRow) - minval;
        else
            break
        end
    end
    %**************************************************************************
    % STEP 5:
    %  Construct a series of alternating primed and starred zeros as
    %  follows:
    %  Let Z0 represent the uncovered primed zero found in Step 4.
    %  Let Z1 denote the starred zero in the column of Z0 (if any).
    %  Let Z2 denote the primed zero in the row of Z1 (there will always
    %  be one).  Continue until the series terminates at a primed zero
    %  that has no starred zero in its column.  Unstar each starred
    %  zero of the series, star each primed zero of the series, erase
    %  all primes and uncover every line in the matrix.  Return to Step 3.
    %**************************************************************************
    rowZ1 = find(starZ==uZc);
    starZ(uZr)=uZc;
    while rowZ1>0
        starZ(rowZ1)=0;
        uZc = primeZ(rowZ1);
        uZr = rowZ1;
        rowZ1 = find(starZ==uZc);
        starZ(uZr)=uZc;
    end
end

% Cost of assignment
rowIdx = find(validRow);
colIdx = find(validCol);
starZ = starZ(1:nRows);
vIdx = starZ <= nCols;
assignment(rowIdx(vIdx)) = colIdx(starZ(vIdx));
cost = trace(costMat(assignment>0,assignment(assignment>0)));

function [minval,rIdx,cIdx]=outerplus(M,x,y)
[nx,ny]=size(M);
minval=inf;
for r=1:nx
    x1=x(r);
    for c=1:ny
        M(r,c)=M(r,c)-(x1+y(c));
        if minval>M(r,c)
            minval=M(r,c);
        end
    end
end
[rIdx,cIdx]=find(M==minval);


function test_suite = test_createBasisTransform
%TEST_CREATEBASISTRANSFORM  Test case for the file createBasisTransform
%
%   Test case for the file createBasisTransform

%   Example
%   test_createBasisTransform
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2011-10-13,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

test_suite = functiontests(localfunctions);

function test_Translate(testCase) %#ok<*DEFNU>
% Basic test to check the function runs

p1 = [3 4];
p2 = [10 20];
basis1 = [p1 1 0 0 1];
basis2 = [p2 1 0 0 1];

dp = p1-p2;
exp = [eye(2) dp' ; 0 0 1];

trans = createBasisTransform(basis1, basis2);
testCase.assertEqual(exp, trans, 'AbsTol', .01);

function test_NonOrthogonalBasis(testCase)
% Check consistency of transfomating to Non Ortho bases, and back to global

src = [0 0  1 0  0 1];
tgt = [1 2  3 4  -5 6];

% transform a polygon to TGT basis
poly = [10 10;30 10; 30 20; 20 20;20 40; 10 40];
trans = createBasisTransform(src, tgt);
poly2 = transformPoint(poly, trans);

% transform back to original basis
trans2 = createBasisTransform(tgt, src);
poly3 = transformPoint(poly2, trans2);

testCase.assertEqual(poly, poly3, 'AbsTol', .01);


%% Pose Estimation
%
% In this sample, we learn to exploit calib3d module to create some 3D effects
% in images from a calibrated camera.
%
% Sources:
%
% * <https://docs.opencv.org/3.2.0/d7/d53/tutorial_py_pose.html>
% * <https://docs.opencv.org/3.2.0/dc/d43/tutorial_camera_calibration_square_chess.html>
%

%%
% In the camera calibration demo, we have found the camera matrix, distortion
% coefficients etc. Given a pattern image, we can utilize the above
% information to calculate its pose, or how the object is situated in space,
% like how it is rotated, how it is displaced etc. For a planar object, we can
% assume Z=0, such that, the problem now becomes how camera is placed in space
% to see our pattern image. So, if we know how the object lies in the space,
% we can draw some 2D diagrams in it to simulate the 3D effect.
%
% Our problem is, we want to draw our 3D coordinate axis (X, Y, Z axes) on our
% chessboard's first corner. X axis in blue color, Y axis in green color and
% Z axis in red color. So in-effect, Z axis should feel like it is
% perpendicular to our chessboard plane.
%

%%
% First, let's load the camera matrix and distortion coefficients from the
% previous calibration result.
fname = fullfile(tempdir(), 'calibration_chessboard.yml');
assert(exist(fname, 'file') == 2, ...
    'calibration result not found, run calibration_demo.m first');
fs = cv.FileStorage(fname);
display(fs)

%%
% Then as in previous case, we create object points
% (planar 3D points of chessboard corners)
[X,Y] = ndgrid(1:fs.board_width, 1:fs.board_height);
objPts = ([X(:), Y(:)] - 1) * fs.square_size;
objPts(:,3) = 0;

%%
% Create axis points. Axis points are points in 3D space for drawing the axis.
% We draw axis of length 3 (units will be in terms of chess square size since
% we calibrated based on that size). So our X axis is drawn from (0,0,0) to
% (3,0,0), so for Y axis. For Z axis, it is drawn from (0,0,0) to (0,0,-3).
% Negative denotes it is drawn towards the camera.
p3d = [3 0 0; 0 3 0; 0 0 -3] * fs.square_size;

%%
% Now, as usual, we loop over each image
files = cv.glob(fullfile(mexopencv.root(), 'test', 'left*.jpg'));
N = numel(files);
for i=1:N
    % load image
    img = cv.imread(files{i}, 'Color',true);

    % search for 9x6 grid
    [imgPts, found] = cv.findChessboardCorners(img, ...
        [fs.board_width, fs.board_height]);
    imgPts = cat(1, imgPts{:});
    if found
        % if found, refine it with subcorner pixels.
        gray = cv.cvtColor(img, 'RGB2GRAY');
        imgPts = cv.cornerSubPix(gray, imgPts, 'WinSize',[11 11], ...
            'Criteria',struct('type','Count+EPS', 'maxCount',30, 'epsilon',0.1));
    end

    % calculate the rotation and translation vectors
    [rvecs, tvecs] = cv.solvePnP(objPts, imgPts, fs.camera_matrix, ...
        'DistCoeffs',fs.distortion_coefficients);

    % use transformation matrices to project "axis points" to the image plane.
    % In simple words, we find the points on image plane corresponding to each
    % of (3,0,0),(0,3,0),(0,0,3) in 3D space.
    p2d = cv.projectPoints(p3d, rvecs, tvecs, fs.camera_matrix, ...
        'DistCoeffs',fs.distortion_coefficients);

    % draw lines from the first corner to each of projected axis points.
    % Notice that each axis is 3 squares long.
    clrs = [0 0 1 0; 0 1 0 0; 1 0 0 0] * 255;
    img = cv.line(img, imgPts([1 1 1],:), p2d, 'Colors',clrs, 'Thickness',3);
    imshow(img), title(sprintf('%02d / %02d', i, N))
    pause(1)
end

%%
% We can also modify the axis points above to draw a cube instead of axis
% vectors. We draw ground floor in green, pillars in blue, top layer in red.
%
% This can be extended to render more complicated objects such as for
% augmented reality applications.
%
p3d = [0 0 0; 0 3 0; 3 3 0; 3 0 0;
    0 0 -3; 0 3 -3; 3 3 -3; 3 0 -3] * fs.square_size;
p2d = cv.projectPoints(p3d, rvecs, tvecs, fs.camera_matrix, ...
    'DistCoeffs',fs.distortion_coefficients);
img = cv.imread(files{N}, 'Color',true);
img = cv.drawContours(img, p2d(1:4,:), 'Color',[0 255 0], 'Thickness','Filled');
img = cv.line(img, p2d(1:4,:), p2d(5:8,:), 'Color',[0 0 255], 'Thickness',3);
img = cv.drawContours(img, p2d(5:8,:), 'Color',[255 0 0], 'Thickness',3);
figure, imshow(img), title(sprintf('%02d / %02d', N, N))


function lambda = lambda_search(x, L, delta, xi)
% lambda = phi^{-1}(delta),
% where phi(lambda)=|min{xi/lambda*E, (L/L+lambda)x}|_F and E=ones(m,n)

    treshold = 1e-12;
    norm_x = norm(x);
    if norm_x <= delta
        lambda = 0;
    else
        n = size(x,1);
        x = abs(x);
        x = sort(x, 'descend');

        if norm_x*(1-xi/(x(1)*L))<delta %This also includes the case: x(k)-xi/L<0
            lambda = L*(norm_x/delta - 1);
        else
            partial_sum = norm_x^2 - x(1)^2;
            flag = 0;
            for k=2:n
                xi_lambda_ratio = x(k)-xi/L;
                if xi_lambda_ratio > 0
                    tail_sum = (k-1)*(xi_lambda_ratio)^2;
                    test_sum = sqrt(tail_sum + (1-xi/(x(k)*L))^2*partial_sum);
                    if test_sum<delta
                        %lambda = L*(sqrt(partial_sum/(delta^2-tail_sum))-1);
                        coef = [delta^2, 2*L*delta^2, (delta^2-partial_sum)*L^2-(k-1)*xi^2, -2*L*(k-1)*xi^2, -(k-1)*xi^2*L^2];
                        lambda = roots(coef);
                        ind=(abs(imag(lambda))<treshold).*(lambda>0);
                        lambda = lambda'*ind;
                        flag = 1;
                        break
                    end
                    partial_sum = partial_sum - x(k)^2;
                else
                    coef = [delta^2, 2*L*delta^2, (delta^2-partial_sum)*L^2-(k-1)*xi^2, -2*L*(k-1)*xi^2, -(k-1)*xi^2*L^2];
                    lambda = roots(coef);
                    ind=(abs(imag(lambda))<treshold).*(lambda>0);
                    lambda = lambda'*ind;
                    flag = 1;
                    break
                end
            end
            if flag == 0
                lambda = sqrt(n)*xi/delta;
            end
        end
    end


function [ n_data, h, a, t ] = owen_values ( n_data )

%*****************************************************************************80
%
%% OWEN_VALUES returns some values of Owen's T function.
%
%  Discussion:
%
%    Owen's T function is useful for computation of the bivariate normal
%    distribution and the distribution of a skewed normal distribution.
%
%    Although it was originally formulated in terms of the bivariate
%    normal function, the function can be defined more directly as
%
%      T(H,A) = 1 / ( 2 * pi ) *
%        Integral ( 0 <= X <= A ) e^(H^2*(1+X^2)/2) / (1+X^2) dX
%
%    In Mathematica, the function can be evaluated by:
%
%      fx = 1/(2*Pi) * Integrate [ E^(-h^2*(1+x^2)/2)/(1+x^2), {x,0,a} ]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 May 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Mike Patefield, David Tandy,
%    Fast and Accurate Calculation of Owen's T Function,
%    Journal of Statistical Software,
%    Volume 5, Number 5, 2000, pages 1-25.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real H, a parameter.
%
%    Output, real A, the upper limit of the integral.
%
%    Output, real T, the value of the function.
%
  n_max = 28;

  a_vec = [ ...
    0.2500000000000000E+00, ...
    0.4375000000000000E+00, ...
    0.9687500000000000E+00, ...
    0.0625000000000000E+00, ...
    0.5000000000000000E+00, ...
    0.9999975000000000E+00, ...
    0.5000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.2000000000000000E+01, ...
    0.3000000000000000E+01, ...
    0.5000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.2000000000000000E+01, ...
    0.3000000000000000E+01, ...
    0.5000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.2000000000000000E+01, ...
    0.3000000000000000E+01, ...
    0.5000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.2000000000000000E+01, ...
    0.3000000000000000E+01, ...
    0.5000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.2000000000000000E+01, ...
    0.3000000000000000E+01, ...
    0.1000000000000000E+02, ...
    0.1000000000000000E+03 ];

  h_vec = [ ...
    0.0625000000000000E+00, ...
    6.5000000000000000E+00, ...
    7.0000000000000000E+00, ...
    4.7812500000000000E+00, ...
    2.0000000000000000E+00, ...
    1.0000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.1000000000000000E+01, ...
    0.1000000000000000E+01, ...
    0.1000000000000000E+01, ...
    0.5000000000000000E+00, ...
    0.5000000000000000E+00, ...
    0.5000000000000000E+00, ...
    0.5000000000000000E+00, ...
    0.2500000000000000E+00, ...
    0.2500000000000000E+00, ...
    0.2500000000000000E+00, ...
    0.2500000000000000E+00, ...
    0.1250000000000000E+00, ...
    0.1250000000000000E+00, ...
    0.1250000000000000E+00, ...
    0.1250000000000000E+00, ...
    0.7812500000000000E-02, ...
    0.7812500000000000E-02, ...
    0.7812500000000000E-02, ...
    0.7812500000000000E-02, ...
    0.7812500000000000E-02, ...
    0.7812500000000000E-02 ];

  t_vec = [ ...
    3.8911930234701366E-02, ...
    2.0005773048508315E-11, ...
    6.3990627193898685E-13, ...
    1.0632974804687463E-07, ...
    8.6250779855215071E-03, ...
    6.6741808978228592E-02, ...
    0.4306469112078537E-01, ...
    0.6674188216570097E-01, ...
    0.7846818699308410E-01, ...
    0.7929950474887259E-01, ...
    0.6448860284750376E-01, ...
    0.1066710629614485E+00, ...
    0.1415806036539784E+00, ...
    0.1510840430760184E+00, ...
    0.7134663382271778E-01, ...
    0.1201285306350883E+00, ...
    0.1666128410939293E+00, ...
    0.1847501847929859E+00, ...
    0.7317273327500385E-01, ...
    0.1237630544953746E+00, ...
    0.1737438887583106E+00, ...
    0.1951190307092811E+00, ...
    0.7378938035365546E-01, ...
    0.1249951430754052E+00, ...
    0.1761984774738108E+00, ...
    0.1987772386442824E+00, ...
    0.2340886964802671E+00, ...
    0.2479460829231492E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    h = 0.0;
    a = 0.0;
    t = 0.0;
  else
    h = h_vec(n_data);
    a = a_vec(n_data);
    t = t_vec(n_data);
  end

  return
end


%  Figure 10.23      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
%  fig10_23.m is a script to generate Fig. 10.23,
%  the root locus of the LQR symmetric root locus compensator of the
%  satellite position control, non-collocated case WITH ESTIMATOR

% Parameter values
m=[1, .1]; k=[0, .091] ; d=[0, .0036]; k1=[0, 0.4];

% call model
[f,g,h,j] = twomass(m,k,d);

% compute feedforward values
s=[f, g;h, 0];
r=[0*g;1];
n=s\r;nx=n(1:4);
nu=n(5);

% call model
[f1,g,h,j] = twomass(m,k1,d);

% form G(s)G(-s) in state-space
a=[f, 0*f;
-h'*h, -f'];
b=[g;0*g];
c=[0*h, g'];
d=[0];
hold off; clf
P=eig(a-b*c*0.1621);
pc=P(real(P<0)==1);
K=place(f,g,pc);
nbar=nu+K*nx;
% eig(f-g*K)
P=eig(a-b*c*3.056e7);
pe=P(real(P<0)==1);
L=place(f',h',pe)';
ac=f-g*K-L*h ;bc=L;cc=K;dc=0;
[Aol,Bol,Col,Dol]=series(ac,bc,cc,dc,f,g,h,j);
[acl,bcl,ccl,dcl]=feedback(f,g,h,j,ac,bc,cc,dc);
[acl1,bcl,ccl,dcl]=feedback(f1,g,h,j,ac,bc,cc,dc);
bcl= nbar*[g;g];
rlocus(Aol,Bol,Col,Dol);
grid;
v=[-2, 2, -1.5, 1.5]
axis(v);
title('Fig. 10.23 Root locus for D_4(s)G(s).')


function [flag] = isHrfStable(X,kas,kaf,phi)

try
    phi;
catch
    phi = -0.5785;
end

% get vasodilatory signal and blood inflow
X1 = X(1);
X2 = X(2);

% if X(1) > 0
%     flag = 1;
%     return
% end

% first compute separatrix
x2 = [-5:1e-3:5]';
n = length(x2);
x1 = phi.*exp(kaf/2-abs(kas-kaf/2)).*exp(x2);
dx1 = diff(x1);
dx2 = diff(x2);

% get closest point on separatrix
d2 = sum([(x1-X1).^2,(x2-X2).^2],2);
[md,ind] = min(d2);

% see whether the point is inside the domain
vec = [x2(ind)-X2,x1(ind)-X1];
tan = [-dx1(ind),dx2(ind)];
nv = sqrt(sum(vec.^2));
nt = sqrt(sum(tan.^2));
angle = acos(sum(vec.*tan)./(nv*nt));
flag = -sign(cos(angle));

hf = figure;
ha = axes(...
    'parent',hf,...
    'nextplot','add');
plot(ha,x2,x1,'.')
plot(ha,x2(ind),x1(ind),'ro')
quiver(x2(1:end-1),...
    x1(1:end-1),...
    -diff(x1),...
    diff(x2),...
    2)
quiver(X2,...
    X1,...
    vec(1),...
    vec(2),...
	'color',[1 0 0])
grid(ha,'on')
axis(ha,'tight')

plot(ha,X2,X1,'r+')
plot(ha,X2,X1,'ro')


function [day_weight]=rmr_day_weight(data_close,data,t1,day_weight, w, epsilon)
%to calculate the day_weight at the t+1 day with robust mean reversion method
%Input:
%    data
%    t1         ---the new day
%    day_weight ---the weight before the new day
%    w          ---the length of window
%    epsilon      ---the parameter to control the reversion threshold
%Output:
%    day_weight ---the weight at the new day
%w=min(w,t1-1);
%x_t1 = l1median_VaZh_z(data((t1-w):(t1-1),:));
if t1<w+2
    x_t1=data(t1-1,:);
else
   x_t1 = l1median_VaZh_z(data_close((t1-w):(t1-1),:))./data_close(t1-1,:);
end
%   if sum(x_t1)==size(data,2)
%       x_t1 = median(data_close((t1-w):(t1-1),:))./data_close(t1-1,:);
%   end

if (norm(x_t1-mean(x_t1)))^2==0
    tao = 0;
else
    tao = min(0,(x_t1*day_weight-epsilon)/(norm(x_t1-mean(x_t1)))^2);
end
day_weight = day_weight - tao*(x_t1-mean(x_t1)*ones(size(x_t1)))';
day_weight = simplex_projection(day_weight,1);

end


function [out] = evap_21(p1,p2,S,Ep,dt)
%evap_21

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Threshold-based evaporation with constant minimum rate
% Constraints:  f <= S/dt
% @(Inputs):    p1   - wilting point (1st threshold) [mm]
%               p2   - 2nd threshold as fraction of wilting point [-]
%               S    - current storage [mm]
%               Ep   - potential evapotranspiration rate [mm/d]
%               dt   - time step size [d]

out = min(max(p2,min(S./p1,1)).*Ep,S./dt);

end


% demo.m

close all;
clear;
clc;

%% set parameters
k = 5;

%% load data
load('./dataset/ORL_Face_img_cov.mat');

%% perform RCM k-NN classifier with
% GRCM2 with eigenvalue-based distance
grcm_accuracy = rcm_knn_classifier(TrainSet, TestSet,'GRCM', '2', 'EV', k);
% RCM4 with eigenvalue-based distance
rcm_accuracy = rcm_knn_classifier(TrainSet, TestSet, 'RCM', '4', 'EV', k);

fprintf('\n');
fprintf('# GRCM2 Accuracy = %5.2f\n', grcm_accuracy);
fprintf('# RCM4 Accuracy = %5.2f\n', rcm_accuracy);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_IRB1200(robot, T)
%   Solves the inverse kinematic problem for the ABB IRB 1200 robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC_IRB1200 returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   abb=load_robot('abb', 'IRB1200');
%   q = [0 0 0 0 0 0];
%   T = directkinematic(abb, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(abb, T);
%   check that all of them are feasible solutions!
%   and every Ti equals T
%   for i=1:8,
%        Ti = directkinematic(abb, qinv(:,i))
%   end
%	See also DIRECTKINEMATIC.
%
%   Author: Arturo Gil Aparicio
%           Universitas Miguel Hernandez, SPAIN.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE 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.
%
% ARTE 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 ARTE.  If not, see <http://www.gnu.org/licenses/>.
function q = inversekinematic_irb1200(robot, T)

%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

% %Evaluate the parameters
% theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
L6=d(6);

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
W = T(1:3,3);

% Pm: wrist position
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
q1=atan2(Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);
%the other possible solution is q1 + pi
q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);
%solver for q3 for both cases
q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%leave only the real part of the solutions
q=real(q);

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60� to -219�, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));

% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1,'algebraic'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'algebraic'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
%theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
%alpha = eval(robot.DH.alpha);

%See geometry
L2=a(2);
L3=d(4);
A2=a(3);

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%The inverse kinematic problem can be solved as in the IRB 140 (for example)

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(-p1(2), p1(1));
gamma = real(acos((L2^2+r^2-L4^2)/(2*r*L2)));

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3
q2(1) = pi/2 - beta - gamma; %elbow up
q2(2) = pi/2 - beta + gamma; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
%theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
%alpha = eval(robot.DH.alpha);

%See geometry
L2=a(2);
L3=d(4);
A2=a(3);

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%the angle phi is fixed
phi=acos((A2^2+L4^2-L3^2)/(2*A2*L4));

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

eta = real(acos((L2^2 + L4^2 - r^2)/(2*L2*L4)));

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = pi - phi- eta;
q3(2) = pi - phi + eta;


% [M_F, V_F] = GPPDE(X_Y, Y, S2_Y, THETA, X_G, G, ALPHA, D, X_F)

% Copyright (c) 2010 Jaakko Luttinen

% TODO: Each equation as a struct/cell: {X, y, K, alpha, D}
%
% X : input
% y : observations
% K : observation covariance matrix
% alpha : coefficients in (differential) equation
% D : differential operators in (differential) equation
%
% Then, noisy function observations are:
% { X_y, y, s2_y, 1, 0 }
% Noiseless derivative observations:
% { X_dy, dy, 1e-8, 1, 1 }
% E.g., heat equation:
% { X_g, zeros(..), 1e-8, [1; -a], [1 0; 0 2] }
%
% Actually, you should allow s2 to be a covariance matrix.  That way you
% could use other unknown functions in equations.. Or maybe even more
% generally, s2 could be a covariance function, thus its hyperparameters
% could be learnt simultaneously. :)
%
% And you can give several of these.
%
% And same for the predictions!

% TODO: Learn hyperparameters.

function [m_f, V_f] = gppde(varargin)

%
% Generate full joint covariance matrix for the equations
%

% Identity matrix for data noise
I_y = speye(size(X_y,1));
alpha0 = 1;
D0 = zeros(1, size(X_y,2));

% Variables for PDE numerical inaccuracy
I_g = speye(length(g));
s2_g = 1e-6;

% Joint data
yg = [y; g];

%
% DEBUG STUFF
%
% $$$ func = @(x) covfunc(x(:)', X_g(1,:), theta, alpha0,D0, alpha, D);
% $$$ mycheckgrad(func,

% Joint prior covariance matrix for observations and PDE
K_g = covfunc(X_g, X_g, theta, alpha, D, alpha, D) + s2_g*I_g;
K_y = covfunc(X_y, X_y, theta, alpha0,D0, alpha0,D0) + s2_y*I_y;
K_y_g = covfunc(X_y, X_g, theta, alpha0,D0, alpha, D);
K_yg = [K_y, K_y_g; K_y_g', K_g];

% $$$ figure
% $$$ imagesc(K_yg)

L_yg = chol(K_yg, 'lower');

% Posterior covariance for predictive function values
K_y_f = covfunc(X_y, X_f, theta, alpha0,D0, alpha0,D0);
K_g_f = covfunc(X_g, X_f, theta, alpha,D, alpha0,D0);
K_yg_f = [K_y_f; K_g_f];
K_f = covfunc(X_f, X_f, theta, alpha0,D0, alpha0,D0);
Z = linsolve_tril(L_yg, K_yg_f);
V_f = K_f - Z' * Z;
m_f = Z' * linsolve_tril(L_yg, yg);

function K = covfunc(X1, X2, theta, alpha1, D1, alpha2, D2)

M = length(alpha1);
N = length(alpha2);
P = size(X1,2);

% For now, distances with unit length scales
D = bsxfun(@minus, ...
           reshape(X1, [size(X1,1),1,P]), ...
           reshape(X2, [1,size(X2,1),P]));

% Unit scale
K0 = theta(1)^2 * exp(-0.5/theta(2)^2*sum(D.^2,3));

K = 0;
for m=1:M
  for n=1:N
    % [c,E] = tmp(D1(m,:)+D2(n,:));

    % Evaluate exponents (E) and coefficients (c)
    b = D1(m,:)+D2(n,:) ;
    A = ntuples(floor(b/2));
    B = repmat(b, [size(A,1),1]);
    E = bsxfun(@minus, b, 2*A) ;
% $$$     c = (-1)^(sum(b)) * (-1).^(sum(A,2)) .* prod(npairsk(B, A), 2) % debug test
    c = (-1)^(sum(D1(m,:))) * (-1).^(sum(A,2)) .* prod(npairsk(B, A), 2) ;

    for l=1:length(c)
      Z = prod(bsxfun(@power, D, reshape(E(l,:), [1,1,P])), 3);
      Z = c(l) * Z;
      thetaexp = sum(0.5*(E(l,:)+b));
      K = K + alpha1(m)*alpha2(n) * theta(2)^(-2*thetaexp) * Z;
    end
  end
end
K = K.*K0;

% $$$ function [c, E] = term_coeff(d)
% $$$
% $$$ Z = ntuples(floor(d/2))
% $$$ D = repmat(d, [size(Z,1),1]);
% $$$ E = bsxfun(@minus, d, 2*Z);
% $$$ c = (-1).^(sum(Z,2)) .* prod(npairsk(D, Z), 2);


function f=ref_iedgtii_1(c,g,a,M)
%REF_EDGTII_1   Reference Inverse Even DGT type II by DGT
%   Usage  c=ref_edgt(f,g,a,M);
%
%   The input window must be odd-centered of length 2L.
%
%   a must be divisable by 2.

L=size(g,1)/2;
W=size(c,2);

N=L/a;

clong=zeros(M,2*N,W);

cr=reshape(c,M,N,W);
% Copy the first half unchanged
clong(:,1:N,:)=cr;

% Copy the non modulated coefficients.
clong(1,N+1:2*N,:)=cr(1,N:-1:1,:);

% Copy the modulated coefficients.
clong(2:M,N+1:2*N,:)=-cr(M:-1:2,N:-1:1,:);

clong=reshape(clong,2*M*N,W);

fdouble=ref_igdgt(clong,g,a,M,.5,0,floor(a/2));

f=fdouble(1:L,:);


classdef SMD10 < PROBLEM
% <multi> <real> <constrained> <bilevel>
% Bilevel optimization problems proposed by Sinha, Malo, and Deb
% maxFElower --- 500 --- Maximum number of lower level function evaluations for each solution

%------------------------------- Reference --------------------------------
% A. Sinha, P. Malo, K. Deb, Test problem construction for single-objective
% bilevel optimization, Evolutionary Computation, 2014, 22(3): 439-477.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties(SetAccess = private)
        maxFElower; % Maximum number of lower level function evaluations for each solution
        DU;         % Number of decision variables of the upper level
        DL;         % Number of decision variables of the lower level
        C;       	% Number of upper constraints
        p;          % The length of xu1
        q;          % The length of xl1
        r;          % The length of xu2 and xl2
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.maxFElower = obj.ParameterSet(500);
            obj.M = 2;
            if isempty(obj.D); obj.D = 5; end
            obj.DU = floor(obj.D/2);
            obj.DL = obj.D - obj.DU;
            obj.C  = obj.DU;
            obj.r  = floor(obj.DU/2);
            obj.p  = obj.DU - obj.r;
            obj.q  = obj.DL - obj.r;
            obj.lower    = [-5*ones(1,obj.DU), -5*ones(1,obj.q), -pi/2*ones(1,obj.r)+1e-6];
            obj.upper    = [10*ones(1,obj.DU), 10*ones(1,obj.q),  pi/2*ones(1,obj.r)-1e-6];
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate upper level and lower level objective values
        function PopObj = CalObj(obj,PopDec)
            xu1 = PopDec(:,1:obj.p);
            xu2 = PopDec(:,obj.p+1:obj.p+obj.r);
            xl1 = PopDec(:,obj.p+obj.r+1:obj.p+obj.r+obj.q);
            xl2 = PopDec(:,obj.p+obj.r+obj.q+1:end);
            % Calculate  UL objective values
            PopObj(:,1) = sum((xu1-2).^2,2) + sum(xl1.^2,2) + sum((xu2-2).^2,2) - sum((xu2-tan(xl2)).^2,2);
            % Calculate  LL objective values
            PopObj(:,2) = sum(xu1.^2,2) + sum((xl1-2).^2,2) + sum((xu2-tan(xl2)).^2,2);
        end
        %% Calculate upper level and lower level constraint violations
        function PopCon = CalCon(obj,PopDec)
            xu1 = PopDec(:,1:obj.p);
            xu2 = PopDec(:,obj.p+1:obj.p+obj.r);
            xl1 = PopDec(:,obj.p+obj.r+1:obj.p+obj.r+obj.q);
            xl2 = PopDec(:,obj.p+obj.r+obj.q+1:end);
            % Upper level constraint violation
            PopCon(:,1:obj.p) = -xu1 - xu1.^3 + sum(xu1.^3,2) + sum(xu2.^3,2);
            PopCon(:,obj.p+1:obj.p+obj.r) = -xu2 - xu2.^3 + sum(xu2.^3,2) + sum(xu1.^3,2);
            % Lower level constraint violation
            PopCon(:,obj.p+obj.r+1:obj.p+obj.r+obj.q) = -xl1 - xl1.^3 + sum(xl1.^3,2);
        end
        %% Calculate lower level objective values
        function llPopulation = EvaluationLower(obj,varargin)
            PopDec            = obj.CalDec(varargin{1});
            PopObj            = obj.CalObj(PopDec);
            PopObj(:,1)       = nan;
            PopCon            = obj.CalCon(PopDec);
            PopCon(:,1:obj.C) = nan;
            llPopulation      = SOLUTION(PopDec,PopObj,PopCon,varargin{2:end});
        end
    end
end


fprintf('\nHere we train an RBM with Continuous inputs (2D Gaussian Mixture).\n');

load('gaussianData.mat');
trainData = data; clear data;
testData = testdata; clear testdata;

arch = struct('size', [2,25], 'classifier',true, 'inputType','gaussian');

opts = {'eta',.01, ...
		'batchSz',64, ...
		'nEpoch',50, ...
		'sparse',0.002, ...
		'displayEvery',10};
%  		'visFun',@visGaussianClassLearning};
arch.opts = opts;

clear r;
r = rbm(arch);
r = r.train(trainData,labels);
[pred,classError,misClass] = r.classify(testData,testlabels);

clf;
scatter(testData(:,1),testData(:,2),[],pred,'.'); colormap lines(3)
hold on
m = plot(testData(misClass,1),testData(misClass,2),'ko');
hold off;
title(sprintf('Predicted classes -- Error=%1.2f %%',classError*100));
legend(m,'Misclassified');


% TEST_CONV_SUB_1D Test case for CONV_SUB_1D
%
% See also
%   CONV_SUB_2D, WAVELET_1D

classdef test_conv_sub_1d < matlab.unittest.TestCase
    methods (Test)
        function testBasic(testcase)
            jsig = 14;
            sig_length = 2^jsig;
            load handel;
            x = y(1:sig_length);
            xf = fft(x);
            ds = randi(jsig)-1;

            white_list = {'filter_format'};
            type = {'d'};
            values = ...
                {{'fourier','fourier_multires','fourier_truncated'}};

            filt_opt = generate_random_options(white_list,type,values)
            filters = morlet_filter_bank_1d(sig_length,filt_opt);
            filter = filters.psi.filter{randi(jsig-ds)};

            for j=1:jsig
                y_old = old_conv_sub_1d(xf,filter,ds); % see below
                y = conv_sub_1d(xf,filter,ds);
                difference = norm(y_old-y)
                is_different = round(10^13*difference);
                assert(is_different==false);
            end
        end
    end
end


function y = obliq(t)

% function to compute mean obliquity of the ecliptic in arcseconds
% capitaine et al. (2003), astronomy and astrophysics 412, 567-586,
% expression from eq. (39) with obliquity at j2000.0 taken from
% eq. (37) or table 8

% input

%  t = tdb julian centuries

% output

%  y = mean obliquity of the ecliptic in arcseconds

% ported from NOVAS 3.0

%%%%%%%%%%%%%%%%%%%%%%%

y = (((( - 0.0000000434d0 * t ...
    - 0.000000576d0) * t ...
    + 0.00200340d0) * t ...
    - 0.0001831d0) * t ...
    - 46.836769d0) * t + 84381.406d0;


function [Pa] = atm2Pa(atm)
% Convert pressure from atmospheres to pascals.
% Chad A. Greene 2012, it's www.chadagreene.com if you're painfully bored.
Pa = atm*101325;


function stroud_test10 ( )

%*****************************************************************************80
%
%% TEST10 tests CIRCLE_CUM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2009
%
%  Author:
%
%    John Burkardt
%
  global FUNC_2D_INDEX;

  r = 3.0;
  xc = 0.0;
  yc = 0.0;

  num = function_2d_num ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST10 \n' );
  fprintf ( 1, '  CIRCLE_CUM approximates an integral over a circle. \n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  We use radius R = %f\n', r );
  fprintf ( 1, '  and center: \n' );
  fprintf ( 1, '  XC = %f\n', xc );
  fprintf ( 1, '  YC = %f\n', yc );
  fprintf ( 1, '\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '    Order:      2             4              8            16 \n' );
  fprintf ( 1, '  F(X) \n' );
  fprintf ( 1, '\n' );

  for i = 1 : num

    FUNC_2D_INDEX = i;

    for j = 1 : 4

      norder = 2^j;

      result(j) = circle_cum ( 'function_2d', xc, yc, r, norder );

    end

    fname = function_2d_name ( i );

    fprintf ( 1, '  %s  %12f  %12f  %12f  %12f\n', fname, result(1:4) );

  end

  return
end


%% A Unit Test Class for confidence interval and fit statistics
classdef confidence_tests < matlab.unittest.TestCase

    properties
        absTol          = 1e-10;
        covAbsTol       = 1e-6;
        confIntAbsTol   = 1e-6;
        confBndsRelTol  = 1e-6;
        pValAbsTol      = 1e-8;
        stdErrRelTol    = 1e-5;
        tStatRelTol     = 1e-5;
        data   = load('Utilities/UnitTests/UnitTestData/confidenceData.mat');
    end

    % Unit Tests
    methods (Test)

        %-- Himmelblau - no weights --%
        function himmelblau(testCase)
            mdl = testCase.data.himmel;
            mdl.fcn = @confidence_tests.himmelblauFcn;
            % Check fit statistics
            checkFitStats(testCase,mdl);
        end

        %-- Himmelblau - with weights --%
        function himmelblauWeights(testCase)
            mdl = testCase.data.himmelw;
            mdl.fcn = @confidence_tests.himmelblauFcn;
            % Check fit statistics
            checkFitStats(testCase,mdl);
        end

        %-- Himmelblau - with lower bound active --%
        function himmelblauLb(testCase)
            mdl = testCase.data.himmellb;
            mdl.fcn = @confidence_tests.himmelblauFcn;
            % Check fit statistics
            checkFitStats(testCase,mdl);
        end

        %-- Himmelblau - with upper bound active --%
        function himmelblauUb(testCase)
            mdl = testCase.data.himmelub;
            mdl.fcn = @confidence_tests.himmelblauFcn;
            % Check fit statistics
            checkFitStats(testCase,mdl);
        end

        %-- Himmelblau - with both bounds active --%
        function himmelblauBothBounds(testCase)
            mdl = testCase.data.himmelbnd;
            mdl.fcn = @confidence_tests.himmelblauFcn;
            % Check fit statistics
            checkFitStats(testCase,mdl);
        end

        %-- Himmelblau - with both bounds and weights --%
        function himmelblauBothBoundsWeights(testCase)
            mdl = testCase.data.himmelbndw;
            mdl.fcn = @confidence_tests.himmelblauFcn;
            % Check fit statistics
            checkFitStats(testCase,mdl);
        end

        %-- SAS - no weights --%
        function sas(testCase)
            mdl = testCase.data.sas;
            mdl.fcn = @confidence_tests.sasFcn;
            % Check fit statistics
            checkFitStats(testCase,mdl);
        end

        %-- SAS - with weights --%
        function sasWeights(testCase)
            mdl = testCase.data.sasw;
            mdl.fcn = @confidence_tests.sasFcn;
            % Check fit statistics
            checkFitStats(testCase,mdl);
        end
    end

    methods
       function checkFitStats(testCase, mdl)
            % Build OPTI object
            optiObj = opti('fun',mdl.fcn, 'data', mdl.xdata, mdl.ydata, 'weights', mdl.wts, 'x0', mdl.sol, 'bounds', mdl.lb, mdl.ub);
            % Manually set the solution
            optiObj.setSolution(mdl.SSE, mdl.sol);
            % Calculate the fit statistics;
            stats  = calcStatistics(optiObj, 0.95, false);

            % Check statistcs
            testCase.verifyEqual(mdl.Rsquare, stats.Rsquare, 'AbsTol', testCase.absTol);
            testCase.verifyEqual(mdl.AdjRsquare, stats.AdjRsquare, 'AbsTol', testCase.absTol);
            testCase.verifyEqual(mdl.RMSE, stats.RMSE, 'AbsTol', testCase.absTol);
            testCase.verifyEqual(mdl.DFE, stats.DFE, 'AbsTol');
            if (isfield(mdl, 'cov'))
                testCase.verifyEqual(mdl.cov, stats.Cov, 'AbsTol', testCase.covAbsTol);
            end
            testCase.verifyEqual(mdl.confInt, stats.ConfInt, 'AbsTol', testCase.confIntAbsTol);
            testCase.verifyEqual(mdl.confBnds, stats.ConfBnds.bnds, 'RelTol', testCase.confBndsRelTol);
            if (isfield(mdl, 'param'))
                testCase.verifyEqual(mdl.param(:,2), stats.Param.StdError, 'RelTol', testCase.stdErrRelTol);
                testCase.verifyEqual(mdl.param(:,3), stats.Param.tStat, 'RelTol', testCase.tStatRelTol);
                testCase.verifyEqual(mdl.param(:,4), stats.Param.pValues, 'AbsTol', testCase.pValAbsTol);
            end
        end
    end

    methods (Static)
        function r = himmelblauFcn(theta,p)
            % r = f(x,theta) (x = pressure, theta unknowns)
            r = theta(1)*p./(1+theta(2)*p);
        end

        function k = sasFcn(theta,n)
            k = theta(1)*n./(theta(2) + n);
        end
    end
end


function h = quiver(v, d, varargin )
%
% Description
% plots the tangential part of a vector field
%
% Syntax
%   quiver(v,d)
%
% Input
%  v - @vector3d
%  d - @vector3d
%
% Options
%  arrowSize     - length of the arrows
%  autoArrowSize - automatically determine the length of the arrows
%  MaxHeadSize   - size of the head
%
% Output
%
% See also

% initialize spherical plot
opt = delete_option(varargin,{'lineStyle','lineColor','lineWidth','color'},1);
sP = newSphericalPlot(v,opt{:},'doNotDraw');

v = vector3d(v);
if length(d) == length(v), d = reshape(d,size(v)); end

mhs = get_option(varargin,'MaxHeadSize',0.9*(1-d.antipodal));
maxD = max(1e-10,max(reshape(norm(d),[],1)));
scale = 0.01 / maxD;
d = d.orthProj(v);

res = min(15*degree,v.resolution);
if isa(sP(1).proj,'plainProjection')
  arrowSize  = get_option(varargin,'arrowSize', 0.75 * res / degree) / maxD;
else
  arrowSize = get_option(varargin,'arrowSize', 0.5 * res / degree) * degree / maxD;
end

for j = 1:numel(sP)

  holdState = get(sP(j).ax,'nextPlot');
  set(sP(j).ax,'nextPlot','add');

  % project data
  [x0,y0] = project(sP(j).proj,normalize(v),'noAntipodal',varargin{:});
  if check_option(varargin,'centered') || mhs == 0

    [x1,y1] = project(sP(j).proj,normalize(v - scale * d),'noAntipodal',varargin{:});
    [x2,y2] = project(sP(j).proj,normalize(v + scale * d),'noAntipodal',varargin{:});

    % we need to rescale to avoid distortions according to projection
    l = sqrt((x1-x0).^2 + (y1-y0).^2);
    x1 = x0 + (x1-x0) ./ l .* norm(d) .* arrowSize / 2;
    y1 = y0 + (y1-y0) ./ l .* norm(d) .* arrowSize / 2;

    x0 = x0 + (x2-x0) ./ l .* norm(d) .* arrowSize / 2;
    y0 = y0 + (y2-y0) ./ l .* norm(d) .* arrowSize / 2;

  else

    [x1,y1] = project(sP(j).proj,normalize(v + 2*abs(scale) * d),'noAntipodal',varargin{:});

    % we need to rescale to avoid distortions according to projection
    l = sqrt((x0-x1).^2 + (y0-y1).^2);
    x1 = x0 + (x1-x0) ./ l .* norm(d) .* arrowSize;
    y1 = y0 + (y1-y0) ./ l .* norm(d) .* arrowSize;

  end

  % if arrowSize==0 the actual length is taken
  if check_option(varargin,'autoArrowSize')
    arrowSizeArg = {};
  else
    arrowSizeArg = {0};
  end

  % make the quiver plot
  varargin = delete_option(varargin,'parent',1);
  h(j) = optiondraw(quiver(x0,y0,x1-x0,y1-y0,arrowSizeArg{:},'MaxHeadSize',mhs,'parent',sP(j).hgt),varargin{:});     %#ok<AGROW>

  % finalize the plot
  % add annotations
  sP(j).plotAnnotate(varargin{:})
  set(sP(j).ax,'nextPlot',holdState);
end

if any(strcmpi(holdState,{'replaceChildren','replace'})) && isappdata(sP(1).parent,'mtexFig')
  mtexFig = getappdata(sP(1).parent,'mtexFig');
  mtexFig.drawNow('figSize',getMTEXpref('figSize'),varargin{:});
end

if nargout == 0, clear('h'); end


function B = pixeldup(A, m, n)
%PIXELDUP Duplicates pixels of an image in both directions.
%   B = PIXELDUP(A, M, N) duplicates each pixel of A M times in the
%   vertical direction and N times in the horizontal direction.
%   Parameters M and N must be integers.  If N is not included, it
%   defaults to M.

%   Copyright 2002-2004 R. C. Gonzalez, R. E. Woods, & S. L. Eddins
%   Digital Image Processing Using MATLAB, Prentice-Hall, 2004
%   $Revision: 1.4 $  $Date: 2003/06/14 16:29:54 $

% Check inputs.
if nargin < 2
   error('At least two inputs are required.');
end
if nargin == 2
   n = m;
end

% Generate a vector with elements 1:size(A, 1).
u = 1:size(A, 1);

% Duplicate each element of the vector m times.
m = round(m); % Protect against nonintergers.
u = u(ones(1, m), :);
u = u(:);

% Now repeat for the other direction.
v = 1:size(A, 2);
n = round(n);
v = v(ones(1, n), :);
v = v(:);
B = A(u, v);


function res = mig(a)
%MIG          Mignitude of point matrix (for completeness)
%
%   res = mig(a)
%

% written  11/23/98     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
%

  res = abs(a);


function kronrod_test02 ( )

%*****************************************************************************80
%
%% KRONROD_TEST02 tests the code for the even case N = 4.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 August 2010
%
%  Author:
%
%    John Burkardt
%
  n = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1,  'KRONROD_TEST02\n' );
  fprintf ( 1,  '  Request KRONROD to compute the Gauss rule\n' );
  fprintf ( 1,  '  of order 4, and the Kronrod extension of\n' );
  fprintf ( 1,  '  order 4+5=9.\n' );

  tol = 0.000001;

  [ x, w1, w2 ] = kronrod ( n, tol );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  KRONROD returns 3 vectors of length %d\n', n + 1 );
  fprintf ( 1, '\n' );
  fprintf ( 1,  '     I      X               WK              WG\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n + 1
    fprintf ( 1, '  %4d  %14f  %14f  %14f\n', i, x(i), w1(i), w2(i) );
  end

  return
end


function q=v_qrdivide(q1,q2)
%V_QRDIVIDE divdes two real quaternions q=[q1,q2]
%
% Inputs:
%
%     q1(4,1), q2(4,1)  Two real quaternions in the form [r, i, j, k]' where i^2=j^2=k^2=ijk=-1
%
% Outputs:
%
%     q(4,1)   Quotient of q1/q2 such that q1=q*q2.
%              Note that q*q2 ~= q2*q since quaternion multiplication does not commute.

%      Copyright (C) Mike Brookes 2000-2008
%      Version: $Id: v_qrdivide.m 10865 2018-09-21 17:22:45Z dmb $
%
%   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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent a b c d
if isempty(a)
    a=[5 8 9 10 15 13];
    b=[6 7 11 12 14 16];
    c=[1 2 3 4 6 7 11 12 16 14];
    d=[1 2 3 4 5 8 9 10 13 15];
end
if nargin<2
    %    just take the inverse of the only input argument
    q=q1/(q1'*q1);
    q(2:4)=-q(2:4);
else
    %    invert q2 and do a multiply
    q=q2/(q2'*q2);
    q(2:4)=-q(2:4);
    t=q1*q.';
    s=zeros(4,4);
    s(a)=-t(b);
    s(c)=t(d);
    q=sum(s,2);
end


% Codes for CVPR-15 work `Face Alignment by Coarse-to-Fine Shape Searching'
% Any question please contact Shizhan Zhu: zhshzhutah2@gmail.com
% Released on July 25, 2015

function [featPCA,pca_model,cut_id] = getPCA(featOri,cut_id)
%[featPCA,pca_model,cut_id] = getPCA(featOri,cut_id)
% pca_model: coeff(n*keep) and meanFeatOri

% fprintf('0');
pca_model.meanFeatOri = double(mean(featOri,1));
assert(size(featOri,1)>=size(featOri,2));
%-----------------------------------------------
if size(featOri,2)>5000
    featOri = single(featOri);
end
[n,p] = size(featOri);
featOri = bsxfun(@minus,featOri,mean(featOri,1));
% parfor i = 1:size(featOri,1),featOri(i,:) = featOri(i,:) - pca_model.meanFeatOri; end;
% fprintf('1');
[U,sigma,coeff] = svd(featOri,0);
clear featOri
% fprintf('2');
sigma = diag(sigma);
% fprintf('3');
score = bsxfun(@times,U,sigma');
clear U;
% fprintf('4');
sigma = sigma ./ sqrt(n-1);
latent = sigma.^2;
clear sigma;
[~,maxind] = max(abs(coeff),[],1);
d = size(coeff,2);
% fprintf('5');
colsign = sign(coeff(maxind + (0:p:(d-1)*p)));
clear maxind;
coeff = bsxfun(@times,coeff,colsign);
score = bsxfun(@times,score,colsign);
clear colsign;
% [coeff,score,latent] = princomp(featOri);

if nargin<2 || isempty(cut_id)
    cut_id = find(cumsum(latent)>sum(latent)*0.98,1);
end

pca_model.coeff = double(coeff(:,1:cut_id));
featPCA = double(score(:,1:cut_id));

end