function show_r3_point_set(points_x,varargin)
%SHOW_R3_POINT_SET 3D illustration of a point set
%
%Syntax
% show_r3_point_set(POINTS_X,options);
%
%Description
% SHOW_R3_POINT_SET(POINTS_X) uses a 3d plot to illustrate a point set in relation to
% the unit sphere S^2.
%
% The argument POINTS_X must be an array of real numbers of size (3 by N), where N is a
% positive integer, representing N points of R^3.
%
% SHOW_R3_POINT_SET(POINTS_X,options) also recognizes a number of illustration
% options, which are specified as name, value pairs.
% Any number of pairs can be used, in any order.
%
% The following illustration options are used.
%
% SHOW_R3_POINT_SET(POINTS_X,'fontsize',size)
% Font size used in titles (numeric, default 16).
%
% SHOW_R3_POINT_SET(POINTS_X,'title','show')
% SHOW_R3_POINT_SET(POINTS_X,'title','hide')
% Show or hide title (default 'hide').
%
% SHOW_R3_POINT_SET(POINTS_X,'sphere','show')
% SHOW_R3_POINT_SET(POINTS_X,'sphere','hide')
% Show or hide the unit sphere S^2 (default 'hide').
%
% For more detail on illustration options, see HELP ILLUSTRATION_OPTIONS.
%
%Note
% This function is primarily for use with the point set POINTS_X as a subset of the
% unit sphere S^2, but this is not assumed and not checked.
% If you show the unit sphere S^2 and POINTS_X contains points closer than radius 1
% from the origin, the sphere will hide these points.
%
%Examples
% > points_x
% points_x =
%          0    0.0000   -0.0000    0.0000
%          0    1.0000   -1.0000         0
%     1.0000    0.0000    0.0000   -1.0000
%
% > show_r3_point_set(points_x,'sphere','hide')
% > show_r3_point_set(points_x,'sphere','show')
%
%See also
% ILLUSTRATION_OPTIONS, SHOW_S2_PARTITION, PROJECT_POINT_SET

% Copyright 2004-2005 Paul Leopardi for the University of New South Wales.
% $Revision 1.10 $ $Date 2005-06-01 $
% Documentation files renamed
% $Revision 1.00 $ $Date 2005-02-12 $
%
% For licensing, see COPYING.
% For references, see AUTHORS.
% For revision history, see CHANGELOG.

gdefault.fontsize = 16;
gdefault.show_title  = false;
gdefault.show_sphere = false;

gopt = illustration_options(gdefault, varargin{:});

N = size(points_x,2);

surf_jet;

if gopt.show_title
    titlestr = sprintf(...
        '\nPoint set containing %d points.',N);
    title(titlestr,'FontWeight','bold','FontUnits','normalized',...
        'FontSize',gopt.fontsize/512);
end

if gopt.show_sphere
    show_s2_sphere;
    hold on
end

[X,Y,Z] = sphere;

r = min(0.05,N^(-1/2)/2);
rX = r*X; rY = r*Y; rZ = r*Z;
for n = 1:N
   surf(points_x(1,n)+rX,points_x(2,n)+rY,points_x(3,n)+rZ,ones(size(rZ)),...
   'FaceColor','interp','FaceLighting','phong','EdgeColor','none')
end
%
axis equal
axis off
grid off
hold off


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function y = MC_merton_sgs(S,r,sigma,T, a,b,lambda,NSim)
%Implements the SGS method for the Merton model
nu = r - lambda*(exp(a+0.5*b^2)-1)-0.5*sigma^2; % martingale correction

X = log(S)*ones(NSim,1);          % X is the log price path

for k=1:NSim
    t = 0;
    tau = [];
    %simulate the jump times first
    while t < T
        dt = -log(rand)/lambda;% jump time
        t = t + dt;            % add the jump times
        tau = [tau; dt];             % not good matlab but works!
    end
    tau(end) = T-(t-dt);
    N = length(tau);                    % N number of jumps + 1
    W1 = randn(1,N);                    % uniforms for diffusion
    if N > 1
        W2 = randn(1,N-1);              % uniforms for jumps
        for i = 1:N-1
            Z = nu*tau(i) + sigma*sqrt(tau(i)) * W1(i);
            lnY = a+b*W2(i);
            X(k) = X(k) + Z + lnY;
        end
    end

    dt_end = tau(end);
    Z = nu * dt_end + sigma * sqrt(dt_end) * W1(end);
    X(k) = X(k) + Z;
end
y = exp(X);
end


function [y alpha eps eta] = sample_int(n, N, p, m, r, Znl, Tnl, Z, T, Hdyn, Zdyn, Tdyn, Rdyn, Qdyn, cdyn, Hmat, Rmat, Qmat, cmat, a1, P1)
% y is p*N*n
% alpha is m*N*n
% eps is p*N*n
% eta is r*N*n

%% Determine nonlinear functions %%
if Znl, Zdyn = false; else Zmat = getmat(Z); end
if Tnl, Tdyn = false; else Tmat = getmat(T); end

%% Draw from Gaussian distribution %%
alpha1  = randn(m, N);
eps     = randn(p, N, n);
eta     = randn(r, N, n);

%% Initialization for sampling %%
if Hdyn, eps(:,:, 1) = sigma_int(Hmat{1}, eps(:,:, 1));
else eps = reshape(sigma_int(Hmat, reshape(eps, p, N*n)), p, N, n); end
if Tdyn, T = Tmat{1}; elseif ~Tnl, T = Tmat; end
if Rdyn, R = Rmat{1}; else R = Rmat; end
if Qdyn, eta(:,:, 1) = sigma_int(Qmat{1}, eta(:,:, 1));
else eta = reshape(sigma_int(Qmat, reshape(eta, r, N*n)), r, N, n); end
if cdyn, c = repmat(cmat{1}, 1, N); else c = repmat(cmat, 1, N); end
P1(P1 == Inf) = 0;
y       = zeros(p, N, n);
alpha   = zeros(m, N, n);

%% Generate independent samples from the model %%
alpha(:,:, 1) = repmat(a1, 1, N) + sigma_int(P1, alpha1);
if Znl, y(:,:, 1) = getfunc(Z, alpha(:,:, 1), 1);
elseif Zdyn, y(:,:, 1) = Zmat{1}*alpha(:,:, 1);
end
for t = 2 : n
    if Hdyn, eps(:,:, t) = sigma_int(Hmat{t}, eps(:,:, t)); end
    if Qdyn, eta(:,:, t) = sigma_int(Qmat{t}, eta(:,:, t)); end
    if Tnl, alpha(:,:, t) = c + getfunc(T, alpha(:,:, t-1), t-1) + R*eta(:,:, t-1);
    else alpha(:,:, t) = c + T*alpha(:,:, t-1) + R*eta(:,:, t-1);
    end
    if Znl, y(:,:, t) = getfunc(Z, alpha(:,:, t), t); end
    if Zdyn, y(:,:, t) = Zmat{t}*alpha(:,:, t); end
    if Tdyn, T = Tmat{t}; end
    if Rdyn, R = Rmat{t}; end
    if cdyn, c = repmat(cmat{t}, 1, N); end
end
if ~Znl && ~Zdyn, y = reshape(Zmat*reshape(alpha, m, N*n), p, N, n) + eps;
else y = y + eps; end

%% Function for incorporating covariance into independent Gaussian samples %%
function x = sigma_int(Sigma, u)
dgSigma = diag(Sigma);
if isequal(Sigma, diag(dgSigma)), x = diag(sqrt(dgSigma))*u;
else % Sigma is not diagonal
    [U Lambda] = eig(full(Sigma));
    x = U*(diag(sqrt(diag(Lambda)))*u);
end


function pix = pixelize(datain_all, intens, box, nx, ny, namefile, showplot)

% pix = pixelize(datain_all, intens, box, nx, ny, namefile,showplot)
% pixelize PALM data
% datain_all - input data, #points-by-2 matrix, each row correspond to xy
% coordinate of the datapoint
% intens - intensities of each center (stored in a0_phot)
% box = [xlim1, xlim2, ylim1, ylim2] - selected ROI
% nx - number of pixels in x direction
% ny - number of pixels in y direction
% namefile - (optional) name of the TXT file with matrix pix
if ~exist ('showplot', 'var')
    showplot = 1;
end

sx = (box(2)-box(1))/nx; %size of the pixels
sy = (box(4)-box(3))/ny;

[datain, indexGood] = ROIdata (datain_all, box(1), box(2), box(3),  box(4), showplot);
intensGood=intens(indexGood);
pixvec = ceil ([(datain(:,1) - box(1))/sx , (datain(:,2) - box(3))/sy ]);

% indrem_min = find (or(pixvec(:,1) < 0, pixvec(:,2) < 0));
% indrem_max = find (or(pixvec(:,1) > nx, pixvec(:,2) > ny));

pix = zeros(nx, ny);
ind = sub2ind(size(pix), pixvec(:,1), pixvec(:,2));
% pix(ind) = intens;
for ii=1:length(ind)
    pix(ind(ii))=pix(ind(ii))+intensGood(ii);
end

% pix = flipud(pix'); % to follow the matlab notation for imagesc(pix)...
pix = pix'; % to follow the matlab notation for imagesc(pix)...

if ~exist ('showplot', 'var')
    showplot = 1;
end

if showplot
    figure
    imagesc(pix)
    set (gca, 'DataAspectRatio',[sy sx 1]);
end

if exist('namefile', 'var')
    if ~isempty(namefile)
        fid = fopen([namefile '.txt'], 'wt'); % Open for writing
        for i=1:size(pix,1)
            fprintf(fid, '%d ', pix(i,:));
            fprintf(fid, '\n');
        end
        fclose(fid);
    end
end


function [y]=col(x, k, varargin)
%Computes the columns of a block TT-tensor
%   [Y]=COL(X,K)
%   It is useful for block algorithms when r(d+1) is not equal to 1
%   For analogous thing on r(1) see ROW(X,K)
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

d=x.d;
r=x.r;

if nargin > 2
    for j = 1 : nargin-2
        k = [k, varargin{j}];
    end
end

BlockSize = r(d+1);
if max(k) > BlockSize
    error('TT-tensor.col:Index exceeds dimensions in TT block.');
end

CutMatr = eye(BlockSize);
CutMatr = CutMatr(:, k);
y = x*CutMatr;

return
end


% Load Alan
x=imread('Alan.jpg','jpg');

% Crop Alan
startx=100;
starty=300;

N=256;
xg=double(x(startx:startx+N-1,starty:starty+N-1,2));
xg=xg-mean(mean(xg));

% Maximum latent space dimension
q=100;
pca=spm_vpca(xg,q);

figure; imagesc(pca.M_w); colormap gray; title('Bayes estimate');
figure; imagesc(pca.ml.W(:,1:q)); colormap gray; title('ML estimate');

figure
plot(pca.Fm_evol);
xlabel('Iterations');
ylabel('Neg. Free Energy');

figure
plot(pca.ml.lambda);
title('Eigenspectrum');

figure
plot(pca.mean_alpha);
title('Prior precision of factors');


%
% step 3: evaluation of performance
%
% using our third set of prices, we estimate dp at each time interval,
% if dp > t and current position <= 0 , we buy
% if dp < -t and current position >= 0, we sell
% else, do:nothing
%

% trade using the above algorithm. returns expected profit
%
% given a list of prices
% assumes k-means clustered patterns have already been calculated
% assumes parameters w_i have already been calculated
% position is 0 or 1 (we have nothing, we have a bitcoin)
% bank is the amount of cash we have
% threshold is the threshold for buying/selling
% defined in the paper
function [error,jinzhi,bank,buy,sell,proba] = brtrade(prices, bidVolume,askVolume, fee)
    assert(exist('thetas.mat','file')==2)
    load('thetas.mat');
    assert(isequal(length(prices), length(bidVolume)));
    assert(isequal(length(prices), length(askVolume)));
    position = 0;
    bank = 0;
    jinzhi = zeros(length(prices)-750, 1);
    error = 0;
    %current error metric is sum(abs(error))/time interval = ~.9
    %current error = 0.06
    buy = [];
    sell = [];
    counttotal = 0;
    counts = 0;
    temp = 0;
    for t = 720:length(prices)-1
        price180 = zscore(prices(t-179:t));
        price360 = zscore(prices(t-359:t));
        price720 = zscore(prices(t-719:t));

        %average price change dp_j is given by bayesian regression
        dp1 = bayesian(price180,kmeans180s);
        dp2 = bayesian(price360,kmeans360s);
        dp3 = bayesian(price720,kmeans720s);

        r = (bidVolume(t)-askVolume(t))/(bidVolume(t)+askVolume(t));

        dp = theta0 +  theta(1)*dp1 + theta(2)*dp2 + theta(3)*dp3;% + theta(4)*r;

        % compare price at t+1 with predicted price jump
        error = error + abs(prices(t+1)-prices(t)-dp);

        % calculate transaction fee??
        % threshold 1 and 2 before...but
        % there is definitely not going to be a
        % 5-8$ price jump predicted in the next ten seconds
        % need to consider TODO
		fee = 0;
		if (fee == 0)
            tfee_buy = 0.001;
            tfee_sell = 0.003;
        else
            tfee_buy = fee*prices(2)/100;
            tfee_sell = tfee_buy;
        end
        %BUY
        if (dp > tfee_buy && position == 0)
            position = 1;
            temp = prices(t);
            fprintf('Buying at %d\n', temp);
            buy = [buy;t];
        end
        %SELL
        if (dp < -tfee_sell && position == 1)
            position = 0;
            bank = bank + prices(t)-temp;
            fprintf('Selling at %d\n', prices(t));
            sell = [sell;t];
            counttotal = counttotal+1;
            if prices(t)-temp>0
                counts = counts+1;
            end
        end

        jinzhi(t) = bank;
    end

    % forces us to close the position at the end
    % tradeoffs to this decision
    % on one side: more realistic
    % but the algorithm doesn't yet account for it
    if (position == 1)
        bank = bank + prices(t)-temp;
        fprintf('Final sale at %d\n', prices(t));
        sell = [sell;t];
        counttotal = counttotal+1;
        if prices(t)-temp>0
            counts = counts+1;
        end
    end
    proba = (counts./counttotal)*100;
    end


function test_ft_networkanalysis

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_networkanalysis

data = [];
for k = 1:5
  x = randn(7,15);
  cx = x*x';
  cx = cx./sqrt(diag(x)*diag(x)');
  data.cohspctrm(:,:,k) = cx;
end
data.freq  = 1:5;
data.label = {'chan1';'chan2';'chan3';'chan4';'chan5';'chan6';'chan7'};
data.dimord = 'chan_chan_freq';
data.cfg    = 'this is the cfg';

tmp = data;
tmp.cohspctrm = data.cohspctrm>0.3;

% at present just checks for undirected binary and weighted graphs
cfg           = [];
cfg.parameter = 'cohspctrm';

cfg.method    = 'assortativity';
stat1 = ft_networkanalysis(cfg, tmp);
stat2 = ft_networkanalysis(cfg, data);

cfg.method    = 'betweenness';
stat3 = ft_networkanalysis(cfg, tmp);
stat4 = ft_networkanalysis(cfg, data);

cfg.method    = 'clustering_coef';
stat5 = ft_networkanalysis(cfg, tmp);
stat6 = ft_networkanalysis(cfg, data);

cfg.method    = 'degrees';
stat7 = ft_networkanalysis(cfg, tmp);
stat8 = ft_networkanalysis(cfg, data);


function a = asinh(a)
%ASINH        Gradient inverse hyperbolic sine asinh(a)
%

% written  10/16/98     S.M. Rump
% modified 10/14/00     S.M. Rump  use Tony's trick
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    accelaration for sparse input
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 10/08/08     S.M. Rump  improved sparse multiplication: not using intval data type
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  N = getappdata(0,'INTLAB_GRADIENT_NUMVAR');
  % use full(a.x(:)): cures Matlab V6.0 bug
  % a=7; i=[1 1]; x=a(i), b=sparse(a); y=b(i)  yields row vector x but column vector y
  % ax is full anyway
  ax = 1 ./ sqrt( 1 + sqr(full(a.x(:))) );
  a.x = asinh(a.x);
  if issparse(a.dx)
    sizeax = size(a.dx,1);
    [ia,ja,sa] = find(a.dx);
    if isa(a.x,'intval')
      adx = times(ax(ia),sa,0);
      if adx.complex
        a.dx = intval( sparse(ia,ja,adx.mid,sizeax,N) , sparse(ia,ja,adx.rad,sizeax,N) , 'midrad' );
      else
        a.dx = intval( sparse(ia,ja,adx.inf,sizeax,N) , sparse(ia,ja,adx.sup,sizeax,N) , 'infsup' );
      end
    else
      a.dx = sparse(ia,ja,ax(ia).*sa,sizeax,N);
    end
  else
    a.dx = a.dx .* ax(:,ones(1,N));
  end

  if rndold
    setround(rndold)
  end


function [iQ] = VBA_inv(Q,indIn,flag,v)
% overloaded sparse matrix pseudo-inverse
% function [iQ] = VBA_inv(Q,indIn,flag,v)
% IN:
%   - Q: the nxn matrix to be inverted
%   - indIn: a vector of indices that specifies the submatrix of Q that has
%   to be inverted. The rest of the matrix is padded with zeros. If empty,
%   the routine looks for infinite or below precision (close to zero)
%   entries in the the diagonal of Q.
%   - flag: if flag='replace', the routine returns Q, having replaced its
%   elements not in 'indIn' with v (see below)
%   - v: a number by which to padd the elements of Q not in 'indIn' (only
%   for flag='replace')
% OUT:
%   - iQ: the nxn matrix that is either the inverse of Q or v-padded Q (for
%   flag='replace').

if nargin < 2 || isempty(indIn)
    dq = diag(Q);
    indIn = find(~isinf(dq)&dq~=0);
end
% use lazy evaluation if all matrix is used
isSub = ~all(numel(indIn) == size(Q));

if nargin < 3
    replace = 0;
else
    replace = isequal(flag,'replace');
end
if nargin < 4
    v = 0;
end
if isSub
    subQ = full(Q(indIn,indIn));
else
    subQ = full(Q);
end
if replace % v-padd
    iQ = v.*ones(size(Q));
    iQ(indIn,indIn) = subQ;
else % (p)invert Q
    if isequal(subQ,eye(length(indIn)))   % identity matrix
        subiQ = subQ;
    elseif isequal(subQ,diag(diag(subQ))) % diagonal matrix
        tol  = max(eps(norm(diag(subQ),'inf'))*length(indIn),exp(-32));
        subiQ = diag((diag(subQ)+tol).^-1);
    else % full matrix
        tol  = max(eps(norm(subQ,'inf'))*length(indIn),exp(-32));
        subiQ = inv(subQ + eye(length(indIn))*tol);
    end
    if isSub
        iQ = zeros(size(Q));
        iQ(indIn,indIn) = subiQ;
    else
        iQ = subiQ;
    end
end


function [ fea, out ] = ex_linearelasticity4( varargin )
%EX_LINEARELASTICITY4 Stress calculation of an I-beam attached to two brackets.
%
%   [ FEA, OUT ] = EX_LINEARELASTICITY4( VARARGIN ) Example to calculate displacements and
%   stresses for an I-beam suppored by two brackets with circular holes.
%
%   Accepts the following property/value pairs.
%
%       Input       Value/{Default}        Description
%       -----------------------------------------------------------------------------------
%       E           scalar {200e9}         Modulus of elasticity
%       nu          scalar {0.3}           Poissons ratio
%       force       scalar {1e5}           Load force
%       l           scalar {0.4}           Length of I-beam
%       ilev        scalar {2}             Grid regfinement level
%       sfun        string {sflag1}        Shape function for displacements
%       iplot       scalar 0/{1}           Plot solution (=1)
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       fea         struct                 Problem definition struct
%       out         struct                 Output struct

% Copyright 2013-2022 Precise Simulation, Ltd.

cOptDef = { ...
  'E',        200e9; ...
  'nu',       0.3; ...
  'force',    1e5; ...
  'l',        0.4; ...
  'ilev',     2; ...
  'sfun',     'sflag1'; ...
  'iplot',    1; ...
  'tol',      0.42; ...
  'fid',      1 };
[got,opt] = parseopt(cOptDef,varargin{:});
fid       = opt.fid;

% Geometry definition.
fea.sdim = { 'x' 'y' 'z' };   % Coordinate names.

% Grid generation.
fea.grid = get_grid( opt.ilev );

% Problem definition.
fea = addphys(fea,@linearelasticity);
fea.phys.el.eqn.coef{1,end} = { opt.nu };
fea.phys.el.eqn.coef{2,end} = { opt.E  };
fea.phys.el.sfun            = { opt.sfun opt.sfun opt.sfun };

% Boundary conditions.
dtol     = sqrt(eps);
fixbdr   = findbdr( fea, ['(sqrt(x.^2+z.^2)<=0.03+sqrt(eps))&(z>=-sqrt(eps))'] );
forcebdr = findbdr( fea, ['abs(y)>=0.2-sqrt(eps)'] );

% Fix boundaries (set zero Dirichlet BCs).
n_bdr  = max(fea.grid.b(3,:));        % Number of boundaries.
bctype = num2cell( zeros(3,n_bdr) );  % First set homogenous Neumann BCs everywhere.
[bctype{:,fixbdr}] = deal( 1 );       % Set Dirchlet BCs for right boundary.
fea.phys.el.bdr.coef{1,5} = bctype;

% Apply negative z-load to left boundary.
bccoef = num2cell( zeros(3,n_bdr) );
[bccoef{3,forcebdr}] = deal(-opt.force);
fea.phys.el.bdr.coef{1,end} = bccoef;

% Parse and solve problem.
fea       = parsephys( fea );
fea       = parseprob( fea );
fea.sol.u = solvestat( fea, 'fid', fid );

% Postprocessing.
if ( opt.iplot>0 )
  DSCALE = 5000;

  subplot(1,2,1)
  postplot( fea, 'surfexpr', 'sqrt(u^2+v^2+w^2)', 'linestyle', 'none' )
  title( 'Total displacement' )
  view([30 20])

  subplot(1,2,2)
  dp = zeros(size(fea.grid.p));
  for i=1:3
    dp(i,:) = DSCALE*evalexpr( fea.dvar{i}, fea.grid.p, fea );
  end
  fea_disp.grid   = fea.grid;
  fea_disp.grid.p = fea_disp.grid.p + dp;
  plotgrid( fea_disp )
  title(['Displacement plot (at ',num2str(DSCALE),' times scale)'])
  view([30 20])

end

% Error check.
disp_max_ref = 6.204e-6;
xdisp = fea.sol.u(fea.eqn.dofm{1}(:));
ydisp = fea.sol.u(fea.eqn.dofm{2}(:)+fea.eqn.ndof(1));
zdisp = fea.sol.u(fea.eqn.dofm{3}(:)+sum(fea.eqn.ndof(1:2)));
disp  = sqrt(xdisp.^2+ydisp.^2+zdisp.^2);
disp_max = max(disp);

svm_max_ref = 4.410e6;
svm = evalexpr( fea.phys.el.eqn.vars{1,2}, fea.grid.p, fea );
svm_max = max(svm);

out.disp_max = disp_max;
out.svm_max  = svm_max;
out.err(1)   = abs(disp_max - disp_max_ref)/abs(disp_max_ref);
out.err(2)   = abs(svm_max - svm_max_ref)/abs(svm_max_ref);
out.pass     = all(out.err<opt.tol);

if ( nargout==0 )
  clear fea out
end

%------------------------------------------------------------------------------%
function [ grid ] = get_grid( ilev )

n0 = 12;
nr = n0*2^(ilev-1);
r  = 0.03;   % Radius of bracket holes.
t  = 0.03;   % Thickness of brackets.
grid01 = ringgrid( 3*2^(ilev-1), 4*nr, r, r+t, [0;0] );
indc01 = selcells( grid01, 'y<=sqrt(eps)' );
grid01 = delcells( grid01, indc01 );

grid02 = holegrid( nr, 3*2^(ilev-1), (r+t)*[-1 1;-1 1], r, [0;0] );
indc02 = selcells( grid02, 'y>=-sqrt(eps)' );
grid02 = delcells( grid02, indc02 );
grid2d = gridmerge( grid01, findbdr(grid01,'y<=sqrt(eps)'), grid02, findbdr(grid02,'y>=-sqrt(eps)') );
t_br = 0.02;   % Width/depth of brackets.
d_br = 0.05;   % Separation distance between brackets.
grid1 = gridextrude( grid2d, 2^(ilev-1), t_br );
grid1 = gridrotate( grid1, pi/2, 1 );
grid2 = grid1;
grid1.p(2,:) = grid1.p(2,:) - d_br/2;
grid2.p(2,:) = grid2.p(2,:) + t_br + d_br/2;

% Create grids for the I-beam.
w_ib = 0.16;   % Beam width.
l_ib = 0.4;    % Beam length.
t_ib = 0.01;   % Beam thickness.
h_ib = 0.1;    % Beam height.

x_in    = linspace(-(r+t),r+t,n0+1);
x_coord = [ -w_ib/2 x_in w_ib/2];
y_coord = [ -0.2 -0.175 -0.15 -0.125 -0.1 -0.075 -(d_br/2+t_br) -d_br/2 0 d_br/2 d_br/2+t_br 0.075 0.1 0.125 0.15 0.175 0.2 ];
for i=2:ilev
  x_coord = sort([ x_coord [x_coord(1:end-1) + x_coord(2:end)]/2 ]);
  y_coord = sort([ y_coord [y_coord(1:end-1) + y_coord(2:end)]/2 ]);
end
grid3 = blockgrid( x_coord, y_coord, 2^(ilev-1), ...
                   [-w_ib/2 w_ib/2;-0.2 0.2;-(r+t)-t_ib -(r+t)] );
tm_ib = 2*(x_in(2)-x_in(1));
grid4 = blockgrid( 2*2^(ilev-1), y_coord, 5*2^(ilev-1), ...
                   [-tm_ib/2 tm_ib/2;-0.2 0.2;-(r+t)-t_ib-h_ib -(r+t)-t_ib] );
grid5 = grid3;
grid5.p(3,:) = grid5.p(3,:) - t_ib - h_ib;

% Merge grids.
tol = sqrt(eps)*1e3;
grid = gridmerge( grid1, findbdr(grid1,['z<=',num2str(-(r+t-tol))]), ...
                  grid3, findbdr(grid3,['z>=',num2str(-(r+t+tol))]) );
grid = gridmerge( grid2, findbdr(grid1,['z<=',num2str(-(r+t-tol))]), ...
                  grid,  findbdr(grid, ['(z>=',num2str(-(r+t+tol)),')&'...
                                        '(z<=',num2str(-(r+t-tol)),')']) );
grid = gridmerge( grid,  findbdr(grid,  ['z<=',num2str(-(r+t+t_ib-tol))]), ...
                  grid4, findbdr(grid4, ['z>=',num2str(-(r+t+t_ib+tol))]), 1 );
grid = gridmerge( grid,  findbdr(grid,  ['z<=',num2str(-(r+t+t_ib+h_ib-tol))]), ...
                  grid5, findbdr(grid5, ['z>=',num2str(-(r+t+t_ib+h_ib+tol))]), 2 );


classdef OptimalControler < handle
    % Let s :=[x(0), .., x(n), u(0)....u(n-1)] be decision variables, then
    % optimal control problem is composed by
    % (i) objective function V(s) = s'*H*S in quadratic form
    % (ii) equality constraints specified by C_eq1 and C_eq2 where C_eq1*s =C_eq2
    % (iii) inequality constraints specified by C_ineq1 and C_ineq2 where C_ineq1 * s <= C_ineq2
    %
    % For potential future extensions, all constraints will be managed by ConstraintManager class.
    % Any time you add a new constraint, those constraint is pushed into the manager with a key (name of constraint).

    properties (SetAccess = private)
        sys; %system
        Xc; Uc; % constraints set for statespace and input space
        x_min; x_max; % lower and upper bound of Xc
        N; % prediction horizon
        Ak; % S.T.M of closed-roop system with LQR feedback
        n_opt; % dim. of optimization parameter s :=[x(0), .., x(n), u(0)....u(n-1)]
        H; % positive definite matrix for objective function V(s) = s'*H*s
        constraint_manager;
    end

    %% Public Methods
    methods (Access = public)

        function obj = OptimalControler(sys, Xc, Uc, N)
            obj.sys = sys;
            obj.Xc = Xc;
            obj.x_min = min(Xc.V, [], 1)';
            obj.x_max = max(Xc.V, [], 2)';
            obj.Uc = Uc;
            obj.N = N;

            obj.n_opt = obj.sys.nx*(obj.N+1)+obj.sys.nu*obj.N;
            obj.constraint_manager = ConstraintManager();

            obj.H = obj.construct_costfunction();
            [C_eq1, C_eq2] = obj.construct_dynamics_constraint();
            [C_ineq1, C_ineq2] = obj.construct_ineq_constraint(Xc, Uc);

            %% Let's change initial and dynamics constraints!!
            obj.constraint_manager.add_eq_constraint('dynamics', C_eq1, C_eq2);
            obj.constraint_manager.add_ineq_constraint('feasible', C_ineq1, C_ineq2);
        end

        function add_initial_eq_constraint(obj, x_init)
            % E * x0 = x_init
            idx_x0_start = 1;
            idx_x0_end = obj.sys.nx;

            C_eq1_init = zeros(obj.sys.nx, obj.n_opt);
            C_eq1_init(:, idx_x0_start:idx_x0_end) = eye(obj.sys.nx);
            C_eq2_init = x_init;
            obj.constraint_manager.add_eq_constraint('initial', C_eq1_init, C_eq2_init);
        end

        function add_terminal_constraint(obj, Xadd)
            [C_ineq1_add, C_ineq2_add] = add_ineq_constraint(obj, Xadd, obj.N+1);
            obj.constraint_manager.add_ineq_constraint('terinal', C_ineq1_add, C_ineq2_add);
        end

        function add_initial_constraint(obj, Xadd)
            [C_ineq1_add, C_ineq2_add] = add_ineq_constraint(obj, Xadd, 1);
            obj.constraint_manager.add_ineq_constraint('initial', C_ineq1_add, C_ineq2_add);
        end

        function [x_seq, u_seq] = solve(obj)
            quadprog_solved = 0;
            [C_eq1, C_eq2] = obj.constraint_manager.combine_all_eq_constraints();
            [C_ineq1, C_ineq2] = obj.constraint_manager.combine_all_ineq_constraints();

            options = optimoptions('quadprog', 'Display', 'none');
            [var_optim, ~, exitflag] = quadprog(obj.H, [], C_ineq1, C_ineq2, C_eq1, C_eq2, [], [], [], options);
            x_seq = reshape(var_optim(1:obj.sys.nx*(obj.N+1)), obj.sys.nx, obj.N+1);
            u_seq = reshape(var_optim(obj.sys.nx*(obj.N+1)+1:obj.n_opt), obj.sys.nu, obj.N);

        end

    end

    %% Methods Used in Constoructor
    methods (Access = private)

        function H = construct_costfunction(obj)
            % compute H
            Q_block = [];
            R_block = [];
            for itr=1:obj.N
                Q_block = blkdiag(Q_block, obj.sys.Q);
                R_block = blkdiag(R_block, obj.sys.R);
            end
            H = blkdiag(Q_block, obj.sys.P, R_block);
        end

        function [C_eq1, C_eq2] = construct_dynamics_constraint(obj)
            % compute C_eq1 and C_eq2
            function C_ss_eq1 = single_step_dynamics_eq1(k)
                % A x(k) - E x(k+1) + Bu(k) = 0
                idx_xk_start = obj.sys.nx * k + 1;
                idx_xk_end = obj.sys.nx * (k + 1);

                idx_xkp1_start = obj.sys.nx * (k + 1) + 1;
                idx_xkp1_end = obj.sys.nx * (k + 2);

                idx_uk_start  = obj.sys.nx * (obj.N+1) + obj.sys.nu * k + 1;
                idx_uk_end  = obj.sys.nx * (obj.N+1) + obj.sys.nu * (k + 1);

                C_ss_eq1 = zeros(obj.sys.nx, obj.n_opt);

                C_ss_eq1(:, idx_xk_start:idx_xk_end) = obj.sys.A;
                C_ss_eq1(:, idx_xkp1_start:idx_xkp1_end) = - eye(obj.sys.nx);
                C_ss_eq1(:, idx_uk_start:idx_uk_end) = obj.sys.B;
            end

            C_eq1 = [];
            for k = 0:obj.N-1
                C_ss_eq1 = single_step_dynamics_eq1(k);
                C_eq1 = [C_eq1; C_ss_eq1];
            end
            C_eq2 = zeros(size(C_eq1, 1), 1);
        end

        function [C_ineq1, C_ineq2] = construct_ineq_constraint(obj, Xc, Uc)
            % compute C_ineq
            [F, G, nc] = convert_Poly2Mat(Xc, Uc);

            F_block = [];
            G_block = [];
            for itr = 1:obj.N
                G_block = blkdiag(G_block, G);
            end
            for itr = 1:obj.N+1
                F_block = blkdiag(F_block, F);
            end
            C_ineq1 = [F_block, [G_block; zeros(nc, obj.sys.nu*obj.N)]];
            nc_total = size(C_ineq1, 1);
            C_ineq2 = ones(nc_total, 1);
        end

    end

    methods (Access = private)

        function [C_ineq1_add, C_ineq2_add] = add_ineq_constraint(obj, Xadd, k_add)
            % add a new constraint at time step k
            if Xadd.contains(zeros(2, 1)) % If Xadd contains the origin, the contraint can be expressed as C1*x<=1
                [F_add, ~, nc_add] = convert_Poly2Mat(Xadd, Polyhedron());
                C_ineq2_add = ones(nc_add, 1);

            else % in other cases, expressed in a general affine form C1*x<=C2
                F_add = Xadd.A;
                nc_add = size(F_add, 1);
                C_ineq2_add = Xadd.b;
            end

            C_ineq1_add = zeros(nc_add, obj.n_opt);
            C_ineq1_add(:, (k_add-1)*obj.sys.nx+1:k_add*obj.sys.nx) = F_add;
        end

    end
end


% Assume a target positioned at x = 1, travelling with speed v = 0.1
state = [1;0.1;2;0;3;0.2;4;0.3];

% Create an instance of a 3D Constant Velocity model
cv = ConstantVelocityX('NumDims',4,'VelocityErrVariance',0.1);

% View the transition matrix and process covariance matrices
F = cv.feval();
Q = cv.covar();

% Predict the target's position and velocity after the interval has passed
newState  = cv.feval(state);

% Do the same as above, but this time add process noise to the prediction
newState2 = cv.feval(state,true);

% Generate 50 random noise samples from the dynamic model
noise = cv.random(50);

% Check how likely the predictions we made are
lik = cv.pdf(newState,state);
lik2 = cv.pdf(newState2,state); % HINT: newState2 should be less likely


sigmoid1 = @(x) 1./(1+exp(-x));

kl_tmp=zeros(1,maxiter);
beta_tmp=zeros(1,maxiter);
dm_tmp=zeros(1,maxiter);
iter=0;
dm=1;
eta=eta0;
m=m+1e-13*randn(1,n);
m=max(m,1e-10);
m=min(m,1-1e-10);
m0 = m;
if (beta>=beta_max), beta=beta_max-1; end;
while ((dm>dmmin)&&(iter<maxiter)&&(beta<beta_max))
	iter=iter+1;
	z=(1-m)./m.*diag(C)';
	C1=C+diag(z);
    v=C1\b;
	w1=v./m';
	betaold=beta;
	beta=1/(sigmay-b'*v);

    mold=m;
    m=(1-eta)*m+eta*sigmoid1(gamma+0.5*p*beta*(w1').^2.*diag(C)');
    m=max(m,1e-10);
    m=min(m,1-1e-10);
    dm=max(abs(m-mold));
    kl_tmp(iter)=-p/2*log(beta)+beta*p/2*(v'*C*v+sum((1-m')./m'.*v.^2.*diag(C))-2*b'*v)+beta*p/2*sigmay- gamma*sum(m)+sum(m.*log(m)+(1-m).*log(1-m));
    if iter>1
        dkl =(kl_tmp(iter)-kl_tmp(iter-1));
        if dkl>1e-10,
            eta=eta/2;
            fprintf('\t[%d] dkl=%e eta=%.3e, dm=%.3e\n',iter,kl_tmp(iter)-kl_tmp(iter-1),eta,dm);
            m=mold;
        end
    end
end;
if iter==maxiter, gamma, iter,
	figure(3)
	plot(1:iter,kl_tmp(1:iter));
end;
kl1=-p/2*log(beta)+beta*p/2*(v'*C*v+sum((1-m')./m'.*v.^2.*diag(C))-2*b'*v)+beta*p/2*sigmay- gamma*sum(m)+sum(m.*log(m)+(1-m).*log(1-m));


function [x,y,z,s,w,flag] = squadsolve(Q,c,A,b,C),
%SQUADSOLVE
%
% USAGE:   [x,y,z,s,w,flag] = squadsolve(Q,c,A,b,C)
%
% PARAMETERS:  Q -> (n,n) symetric matrix (definite positive)
%              c -> (n,1) vector
%              A -> (m,n) matrix
%              b -> (m,1) vector
%              C -> scalar
%
%            x -> primal variables
%            y -> lagrangian coeff of equality constraints
%            z -> dual variables of x
%            s -> primal auxiliary variable (only if C < Inf)
%            w -> dual variable of s
%            flag -> set to 0 => no problem, set to 1 => problem
%
% DESCRIPTION: Primal-dual method for quadratic programming
%
%            minimize c'*x + 0.5*x'*Q*x
%
%            subject to  A*x=b
%                        0<= x <= C
%            The method used here is a primal dual method with a predictor-corrector
%            approach and a logarithmic barrier. I used the heuristic from two
%            existing methods LOQO and HOPDM. The method is an iterative method. The
%            maximal number of iteration is stored in the variable 'max_iter'.
%
% ERRORS AND BUGS: there is no test about the conditionning of the matrix Q. If the iteration
%                  50 has been reached, then the optimization may not be finished and the output
%                  may be wrong.
%
% NOTES: 50 iterations have always been sufficient to solve all problems.
%        This code should be read with the tech. report:
%                "Regularized Symmetric Indefinite Systems in Interior Point
%                 Methods for Linear and Quadratic Optimization",
%                 A. Altman and J. Gondzio, Logilab Tech. Report 1998.6
%
% Andre Elisseeff, Dec. 1999
% aelissee@eric.univ-lyon2.fr

 disp('toto');
% init
  verbose = 1;
  n = size(Q,1);
  m = size(A,1);
  H = zeros(n+m,n+m);
  flag=1;

% Values of the original HOPDM of Gondzio and Altmann
  dinf = 10^(-14);
  smallz = 10^(-14);
  smallt = 2.3*10^(-16);
  opttol = 10^(-5)/C;
  itref = 1;
  mu = 1;
  maxiter = 50;

% init values of the primal and dual variables
  x=ones(n,1);
  z=ones(n,1);
  y=ones(m,1);
  if C < Inf,
    s=ones(n,1);
    w=ones(n,1);
  else
    s=[];w=[];
  end;
% Description of variables:
%
%    x,s     -> primal variables
%    z,w     -> dual variables
%    n       -> number of variables in the initial pb (size of x)
%    m       -> number of constraints in A
%
%    dinf     -> smallest value for all variables
%    smallz   -> smallest value of z
%    smallt   -> smallest value for t in the computation of the matrix theta
%    opttol   -> acceptable tolerance for optimality conditions
%    itref    -> iteration counter
%    maxiter  -> maximum number of iteration
%    DEBUG    -> 0 => no debug, 1 => debug

% Analyze the constraints...
  if verbose,
   disp(sprintf('Analyzing the equality constraints...\n'));
  end;
  [QQ,RR]=qr(A',0);
  [mm,nb] = size(QQ); %% number of eq constraints
  ind = 1:1:nb;
  for i=1:nb,
    if abs(RR(i,i)) < 10^(-10),
      disp(sprintf('Constraints %d removed because of dependence\n',i));
      ind(i)=0;
    end;
  end;
  indice = find(ind >0);

  if (isempty(indice))
    disp(sprintf('No equality constraints... \n'));
    A=[];
    m=0;
  else
    A = A(indice,:); %% new independent eq constraints
    b = b(indice);
    y = y(indice);
    m=length(indice);
  end;
  clear QQ;clear RR;
%%%% AE : scale the problem, has been tested and seems to work
%%%% better : more stable...
  if C < Inf,
    u = ones(n,1);
    Q = Q*C;
    b = b/C;
    c = c;
  end;
% init values before looping
  cont = 1;
  objQ = 0.5*x'*Q*x;
% init values of primal and dual objective functions
  pobjo = abs(c'*x+objQ) + 1;
  if C < inf,
    dobjo = abs(b'*y - u'*w - objQ);
  else
    dobjo = abs(b'*y - objQ);
  end;
%%%%%%%%%%%%%%%
%% MAIN LOOP
%%%%%%%%%%%%%%%
  while (cont)&(itref<=maxiter)
  % Compute the primal objective function
    objQ = 0.5*x'*Q*x;
    pobj = c'*x+objQ;
    if (C<Inf)
        if ~isempty(A),
            dobj = b'*y - u'*w - objQ;
        else
            dobj = - u'*w - objQ;
        end;
    else
        if ~isempty(A),
            dobj = b'*y - objQ;
        else
            dobj = - objQ;
        end;
    end;
    dlgap = pobj - dobj;
    dp = abs(pobj)/(abs(pobjo)+1);
    dd = abs(dobj)/(abs(dobjo)+1);
    dobjo = dobj;
    pobjo = pobj;
    if verbose,
        disp(sprintf('%d - pobj : %f - dobj : %f\n',itref,pobj,dobj));
    end;
  % Check if the solution are bounded
    if (dp > 10^6)
      disp(sprintf('Solution not bounded in the primal. Exit.\n'));
      return;
    end;
    if (dd > 10^6)
      disp(sprintf('Solution not bounded in the dual. Exit.\n'));
      return;
    end;

  % test if optimality
    oldgap = dlgap;
    dp = abs(dobj) + 1;
    if ((abs(dlgap)/dp) <= opttol)
      if verbose,
       disp(sprintf('Optimal solution found. Exit.\n'));
      end;
      cont = 0;
      break;
    end;

    dp = dp + abs(pobj);
    T = abs(dlgap)/dp;

  % put the variables away from zero (from HOPDM)
    if (itref <= 3)
      ax = 2*10^(-3);
      az = 10^(-3);
    elseif (T >= 0.8)
      ax = 2*10^(-4);
      az = 10^(-4);
    elseif (T >= 0.1)
      ax = 2*10^(-5);
      az = 10^(-5);
    elseif (T >=0.01)
      ax = 2*10^(-6);
      az = 10^(-6);
    elseif (T>=0.001)
      ax = 2*10^(-7);
      az = 10^(-7);
    elseif (T>=0.0001)
      ax = 2*10^(-8);
      az = 10^(-7);
    elseif (T>=0.00001)
      ax = 2*10^(-9);
      az = 10^(-9);
    else
      ax = T*10^(-5);
      az = ax;
    end;

  % consider only variables that can be changed
    x = x + ax;
    z = z + az;
    if C < Inf
      s = s + ax;
      w = w + az;
    end;
  % Compute the values of xi_b, xi_c and xi_u
    if ~isempty(A),
     xi_b = -A*x + b;
     xi_c = c - A'*y - z + Q*x;
    else
      xi_b = [];
      xi_c = c - z + Q*x;
     end;

    xi_z =   - x.*z;
    if C < Inf,
      xi_c=xi_c + w;
      xi_u = u - x - s;
      xi_w =  - s.*w;
    end;
  % Compute theta = (z/x + w/s)

  % for bounded variables
    if C<Inf,
      dp = x;
      if (max(abs(dp))<= smallz)
        disp(sprintf('Conditioning problem to invert theta. Abort.\n'));
        return;
      end;
      dpp= s;
      if (max(abs(dpp))<= smallz)
        disp(sprintf('Conditioning problem to invert theta. Abort.\n'));
        return;
      end;
      theta=z./dp + w./dpp;
    end;
  % for unbounded variables
    if C==Inf,
      dp = x;
      if (max(abs(dp))<= smallz)
        disp(sprintf('Conditioning problem to invert theta. Abort.\n'));
        return;
      end;
      theta = z./dp;
    end;

  % neglect small elements of theta array

    neglect = find(theta < smallt);
    if ~isempty(neglect)
      theta(neglect)=zeros(size(neglect));
    end;

  % and control large elements of theta

    neglect = find(theta >= 10^8);
    if ~isempty(neglect)
      theta(neglect)=(10^4)*sqrt(theta(neglect));
    end;
  % factorize H = [-Q-theta^(-1)   A^T]
  %               [ A               0 ]

    H = zeros(n+m,n+m);
    H(1:n,1:n) = -Q-diag(theta);
    H(n+1:n+m,1:n) = A;
    H(1:n,n+1:n+m) = A';

  % Compute the predictor step
    if C < Inf,
      f = xi_c-xi_z./x+(xi_w - xi_u.*w)./s ;
      h = xi_b;
    else
      f = xi_c - xi_z./x;
      h = xi_b;
    end;
    delta=H\[f;h];
    dx = delta(1:n);
    dy = delta(n+1:n+m);
    dz = (xi_z-z.*dx)./x;
    if C<Inf,
      ds = xi_u - dx;
      dw = (xi_w-w.*ds)./s;
    end;

 % determine the maximum step size alpha_p (primal) and
 % alpha_d (dual) to stay in feasible region
 % (x,s,z,w must be positive and greater than dinf)
      indz = find(dz<0);
      indx = find(dx<0);
      inds=[];mins=1;
      indw=[];minw=1;
      if C < Inf,
        inds = find(ds<0);
        indw = find(dw<0);
        if ~isempty(inds)
          mins = min(-(s(inds)-dinf)./ds(inds));
        else
         mins = 1;
        end;
        if ~isempty(indw)
          minw = min(-(w(indw)-dinf)./dw(indw));
        else
         minw = 1;
        end;
      end;
      if ~isempty(indx)
        minx = min(-(x(indx)-dinf)./dx(indx));
      else
        minx = 1;
      end;
      apk = min([minx,mins,1]);
      if ~isempty(indz),
        minz = min(-(z(indz)-dinf)./dz(indz));
      else
        minz = 1;
      end;
      adk = min([minw,minz,1]);

      ax = sum(x.*z);
      as = sum((x+apk*dx).*(z+adk*dz));
      az = sum(dx.^2+dz.^2);
      if C < Inf,
        ax = ax + sum(s.*w);
        as = as + sum((s+apk*ds).*(w+adk*dw));
        az = az + sum(ds.^2+dw.^2);
      end;
    % check if complementary gap is less than opttol
      if (as <= opttol)
        if verbose,
            disp(sprintf('Complementary gap is less than %f\n',opttol));
        end;
        cont = 0;
        x = x + apk*dx;
        y = y + adk*dy;
        z = z + adk*dz;
        if C < Inf,
         s = s + apk*ds;
         w = w + adk*dw;
        end;
        break;
      end;

    % Set the barrier parameter : LOQO's heuristic
      ap = min(apk,adk);
      mu = (ax/(2*n))*(0.95*(1/ap) -1)^2/(0.95*(1/ap)+10)^2;

    % Compute the new direction (algo. of Mehrotra) of order 1 (corrector step)
      xi_z =  - x.*z + mu*ones(size(x)) - dx.*dz;
      f = xi_c - xi_z./x;
      if C < Inf
        xi_w = - s.*w + mu*ones(size(s)) - ds.*dw;
        f=f+ xi_w./s - (w.*xi_u)./s;
      end;
      h = xi_b;

      delta=H\[f;h];
      dx = delta(1:n);
      dy = delta(n+1:n+m);
      dz = (xi_z-z.*dx)./x;
      if C<Inf,
        ds = xi_u - dx;
        dw = (xi_w-w.*ds)./s;
      end;

    % determine the maximum step size alpha_p (primal) and
    % alpha_d (dual) to stay in feasible region
    % (x,s,z,w must be positiv)
      indz = find(dz<0);
      indx = find(dx<0);
      if C < Inf,
        inds = find(ds<0);
        indw = find(dw<0);
        if ~isempty(inds)
          mins = min(-s(inds)./ds(inds));
        else
      mins=1;
        end;
        if ~isempty(indw)
          minw = min(-w(indw)./dw(indw));
        else
      minw=1;
        end;
      end;
      if ~isempty(indx)
        minx = min(-x(indx)./dx(indx));
      else
        minx=1;
      end;
      alpha_p = min([minx,mins,1]);
      if ~isempty(indz)
        minz = min(-z(indz)./dz(indz));
      else
        minz=1;
      end;
      alpha_d = min([minw,minz,1]);

    % Compute step factors
      fp =0.9*min(alpha_p,alpha_d);
      fd =0.9*min(alpha_d,alpha_p);
      x = x +fp*dx;
      y = y +fd*dy;
      z= z + fd*dz;
      if C < Inf,
        w = w + fd*dw;
        s = s +fp*ds;
      end;
      itref=itref+1;
  end;
%%%%%%%%%%%%%%%%%%%%%
%%% End of main loop
%%%%%%%%%%%%%%%%%%%%%

    if (cont==0)
        if verbose,
          disp(sprintf('Optimal Solution found after %d iteration.\n',itref));
          disp(sprintf('Value of the objective : %f.\n',pobj));
        end;
      flag=0;
    end;
  % rescale x and y
    if C<Inf,
      x= C*x;
      %y = C*y;
    end;

end;


%gaussian mixture model
%>
%> @param FeatureMatrix: features for all train observations (dimension iNumFeatures x iNumObservations)
%> @param k: number of gaussians
%> @param numMaxIter: maximum number of iterations (stop if not converged before)
%> @param prevState: internal state that can be stored to continue clustering later
%>
%> @retval mu means
%> @retval sigma standard deviations
%> @retval state result containing internal state (if needed)
% ======================================================================
function [mu, sigma, state] = ToolGmm(V, k, numMaxIter, prevState)

    if (nargin < 3)
        numMaxIter  = 1000;
    end
    if (nargin == 4)
        state = prevState;
    else
        % initialize state
        state = initState_I(V, k);
    end

    for j = 1:numMaxIter
        prevState = state;

        % compute weighted gaussian
        p = computeProb_I(V, state);

        % update clusters
        state = updateGaussians_I(V, p, state);

        % if we have converged, break
        if (max(sum(abs(state.m-prevState.m))) <= 1e-20)
            break;
        end
    end

    mu = state.m;
    sigma = state.sigma;
end

function [state] = updateGaussians_I(FeatureMatrix, p, state)

    % number of clusters
    K = size(state.m, 2);

    % update priors
    state.prior = mean(p, 1)';

    for k = 1:K
        s = 0;

        % update means
        state.m(:, k) = FeatureMatrix * p(:, k) / sum(p(:, k));

        % subtract mean
        F = FeatureMatrix - repmat(state.m(:, k), 1, size(FeatureMatrix, 2));

        for n = 1:size(FeatureMatrix, 2)
            s = s + p(n, k) * (F(:, n) * F(:, n)');
        end
        state.sigma(:, :, k) = s / sum(p(:, k));
    end
end

function [p] = computeProb_I(FeatureMatrix, state)

    K = size(state.m, 2);
    p = zeros(size(FeatureMatrix, 2), K);

    % for each cluster
    for k = 1:K
        % subtract mean
        F = FeatureMatrix - repmat(state.m(:, k), 1, size(FeatureMatrix, 2));

        % weighted gaussian
        p(:, k) = 1 / sqrt((2*pi)^size(F, 1) * det(state.sigma(:, :, k))) *...
            exp(-1/2 * sum((F .* (inv(state.sigma(:, :, k)) * F)), 1)');
        p(:, k) = state.prior(k) * p(:, k);
    end

    % norm over clusters
    p = p ./ repmat(sum(p, 2), 1, K);
end

function [state] = initState_I(FeatureMatrix, K)

    %init
    m       = zeros(size(FeatureMatrix, 1), K);
    sigma   = zeros(size(FeatureMatrix, 1), size(FeatureMatrix, 1), K);
    prior   = zeros(1, K);

    % pick random points as cluster means
    mIdx    = round(rand(1, K) * (size(FeatureMatrix, 2)-1)) + 1;

    % assign means etc.
    m       = FeatureMatrix(:, mIdx);
    prior   = ones(1, K) / K;
    sigma   = repmat(cov(FeatureMatrix'), 1, 1, K);

    % write initial state
    state   = struct('m', m, 'sigma', sigma, 'prior', prior);
end


function fh = decomp_reconst_full(im,Nsc,Nor,block,noise,parent,covariance,optim,sig);

% Decompose image into subbands, denoise, and recompose again.
%		fh = decomp_reconst(im,Nsc,Nor,block,noise,parent,covariance,optim,sig);
%       covariance:	 are we considering covariance or just variance?
%       optim:		 for choosing between BLS-GSM (optim = 1) and MAP-GSM (optim = 0)
%       sig:        standard deviation (scalar for uniform noise or matrix for spatially varying noise)
% Version using the Full steerable pyramid (2) (High pass residual
% splitted into orientations).

% JPM, Univ. de Granada, 5/02
% Last Revision: 11/04

if (block(1)/2==floor(block(1)/2))|(block(2)/2==floor(block(2)/2)),
   error('Spatial dimensions of neighborhood must be odd!');
end

if ~exist('parent'),
        parent = 1;
end

if ~exist('covariance'),
        covariance = 1;
end

if ~exist('optim'),
        optim = 1;
end

if ~exist('sig'),
        sig = sqrt(mean(noise.^2));
end

[pyr,pind] = buildFullSFpyr2(im,Nsc,Nor-1);
[pyrN,pind] = buildFullSFpyr2(noise,Nsc,Nor-1);
pyrh = real(pyr);
Nband = size(pind,1)-1;
for nband = 2:Nband, % everything except the low-pass residual
  fprintf('%d % ',round(100*(nband-1)/(Nband-1)))
  aux = pyrBand(pyr, pind, nband);
  auxn = pyrBand(pyrN, pind, nband);
  [Nsy,Nsx] = size(aux);
  prnt = parent & (nband < Nband-Nor);   % has the subband a parent?
  BL = zeros(size(aux,1),size(aux,2),1 + prnt);
  BLn = zeros(size(aux,1),size(aux,2),1 + prnt);
  BL(:,:,1) = aux;
  BLn(:,:,1) = auxn*sqrt(((Nsy-2)*(Nsx-2))/(Nsy*Nsx));     % because we are discarding 2 coefficients on every dimension
  if prnt,
  	aux = pyrBand(pyr, pind, nband+Nor);
    auxn = pyrBand(pyrN, pind, nband+Nor);
    if nband>Nor+1,     % resample 2x2 the parent if not in the high-pass oriented subbands.
	   aux = real(expand(aux,2));
       auxn = real(expand(auxn,2));
    end
  	BL(:,:,2) = aux;
    BLn(:,:,2) = auxn*sqrt(((Nsy-2)*(Nsx-2))/(Nsy*Nsx)); % because we are discarding 2 coefficients on every dimension
  end

  sy2 = mean2(BL(:,:,1).^2);
  sn2 = mean2(BLn(:,:,1).^2);
  if sy2>sn2,
     SNRin = 10*log10((sy2-sn2)/sn2);
  else
     disp('Signal is not detectable in noisy subband');
  end

  % main
  BL = denoi_BLS_GSM_band(BL,block,BLn,prnt,covariance,optim,sig);
  pyrh(pyrBandIndices(pind,nband)) = BL(:)';
end
fh = reconFullSFpyr2(pyrh,pind);


% change in body orientation
function [data,units] = compute_dtheta(trx,n)

flies = trx.exp2flies{n};
nflies = numel(flies);
data = cell(1,nflies);
for i = 1:nflies,
  fly = flies(i);
  if trx(fly).nframes <= 1,
    data{i} = [];
  else
    data{i} = modrange(diff(trx(fly).theta_mm,1,2),-pi,pi)./trx(fly).dt;
  end
end
units = parseunits('rad/s');


function [cd, score] = distance_metrics(model_gt, model, theta)

if nargin < 3
    theta = 0.1;
end

if ~isfield(model_gt, 'vtx')
    [model_gt.vtx] = model_gt.vertices;
    model_gt = rmfield(model_gt,'vertices');
    [model_gt.mesh] = model_gt.faces;
    model_gt = rmfield(model_gt,'faces');
end

% compare cad model
% normalize both models
mask = (model_gt.anchor~=0)&(model.anchor~=0);
mean_est = mean(model.vtx(model.anchor(mask), :), 1);
if isequal(size(model_gt.vtx,1), 36) && isequal(numel(model_gt.anchor), 8) % TODO
    model_gt.anchor(2) = 36;
    mean_gtr = mean(model_gt.vtx(model_gt.anchor(mask), :), 1);
else
    mean_gtr = mean(model_gt.vtx(model_gt.anchor(mask), :), 1);
end
model.vtx = bsxfun(@minus, model.vtx, mean_est);
model_gt.vtx = bsxfun(@minus, model_gt.vtx, mean_gtr);

std_est = mean(std(model.vtx(model.anchor(mask), :), 1, 1));
std_gtr = mean(std(model_gt.vtx(model_gt.anchor(mask), :), 1, 1));
model.vtx = model.vtx/std_est;
model_gt.vtx = model_gt.vtx/std_gtr;

R = align_models(model, model_gt);
model.vtx = model.vtx*R';

model = computeMeshInfo(model);
model_gt = computeMeshInfo(model_gt);

% compute distance from model to target
kdtree = KDTreeSearcher(model_gt.vtx);
[U,~] = surfProjection(model, model_gt, kdtree);
dist_model_target = sum(sum((model.vtx - U).^2, 2)) / size(model.vtx, 1);

dist_model_target_theta = sum(sum((model.vtx - U).^2, 2) > theta)...
    / size(model.vtx, 1);

% compute distance from target to model
kdtree = KDTreeSearcher(model.vtx);
[U,~] = surfProjection(model_gt, model, kdtree);
dist_target_model = sum(sum((model_gt.vtx - U).^2, 2)) / size(model_gt.vtx, 1);

dist_target_model_theta = sum(sum((model_gt.vtx - U).^2, 2) > theta) ...
    / size(model_gt.vtx, 1);

cd = dist_model_target + dist_target_model;
score = dist_model_target_theta + dist_target_model_theta;


function distance = getStructureDistance(structNum1,structNum2,planC)
%distance = getStructureDistance.m(structNum1,structNum1,planC)
%
%This function returns the distance(cm) between center of masses of
%structNum1 and structNum1
%
%APA, 10/08/2009
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

%Check if plan passed, if not use global.
if ~exist('planC')
    global planC;
end
indexS = planC{end};

%Compute centroid of structNum1
[x1,y1,z1] = calcIsocenter(structNum1, 'COM', planC);

%Compute centroid of structNum2
[x2,y2,z2] = calcIsocenter(structNum2, 'COM', planC);

%Compute distance between centroid1 and centroid2
distance = sqrt((x1-x2)^2 + (y1-y2)^2 + (z1-z2)^2);


function bw2 = hough_bin_pixels(bw, theta, rho, bin)
%HOUGH_BIN_PIXELS Find pixels corresponding to Hough accumulator bin.
%   BW2 = hough_bin_pixels(BW, THETA, RHO, BIN) finds the white pixels in a
%   binary image that correspond to a particular Hough transform
%   accumulator bin.  BW is the original binary image.  THETA and RHO are
%   the Hough parameter vectors returned by the hough function.  BIN is a
%   two-element vector containing the row-column coordinates of the Hough
%   transform bin.  BW2 is a binary image containing only the white pixels
%   in BW that contributed to the specified bin.
%
%   Example
%   =======
%   I  = imread('circuit.tif');
%   BW = edge(I,'canny');
%   imshow(BW)
%   [H,theta,rho] = hough(BW);
%   P = houghpeaks(H, 1);
%   BW2 = hough_bin_pixels(BW, theta, rho, P);
%   figure, imshow(BW2)
%   title('Pixels corresponding to maximum Hough transform bin')
%
%   See also hough, houghlines, houghpeaks.

%   Steven L. Eddins
%   The MathWorks, Inc.

[y, x] = find(bw);
x = x - 1;
y = y - 1;

theta_c = theta(bin(2)) * pi / 180;
rho_xy = x*cos(theta_c) + y*sin(theta_c);
nrho = length(rho);
slope = (nrho - 1)/(rho(end) - rho(1));
rho_bin_index = round(slope*(rho_xy - rho(1)) + 1);

idx = find(rho_bin_index == bin(1));

r = y(idx) + 1;
c = x(idx) + 1;

bw2 = false(size(bw));

bw2(sub2ind(size(bw), r, c)) = true;


function [l,d,perm] = mchol(A,mu)
% [l,d,perm] = mchol(A,mu)
% Compute the Gill-Murray modified LDL factorization of A,

if nargin < 2
    mu = 1e-12;
end

n = size(A,1);
l = eye(n);
d = zeros(n,1);
perm = 1:n;

for i = 1:n
    c(i,i) = A(i,i);
end

% Compute modification parameters
gamma = max(abs(diag(A)));
xi = max(max(abs(setdiag(A,0))));
delta = mu*max(gamma+xi,1);
if n > 1
    beta = sqrt(max([gamma xi/sqrt(n^2-1) mu]));
else
    beta = sqrt(max([gamma mu]));
end

for j = 1:n

    % Find q that results in Best Permutation with j
    [maxVal maxPos] = max(abs(diag(c(j:end,j:end))));
    q = maxPos+j-1;

    % Permute d,c,l,a
    d([j q]) = d([q j]);
    perm([j q]) = perm([q j]);
    c([j q],:) = c([q j],:);
    c(:,[j q]) = c(:,[q j]);
    l([j q],:) = l([q j],:);
    l(:,[j q]) = l(:,[q j]);
    A([j q],:) = A([q j],:);
    A(:,[j q]) = A(:,[q j]);

    for s = 1:j-1
        l(j,s) = c(j,s)/d(s);
    end
    for i = j+1:n
        c(i,j) = A(i,j) - sum(l(j,1:j-1).*c(i,1:j-1));
    end
    theta = 0;
    if j < n
        theta = max(abs(c(j+1:n,j)));
    end
    d(j) = max([abs(c(j,j)) (theta/beta)^2 delta]);
    if j < n
        for i = j+1:n
            c(i,i) = c(i,i) - (c(i,j)^2)/d(j);
        end
    end
end


function [N,K,D,L,var,w_max,w_min,c1,c2,position,p_best,g_best,fitness,p_best_fit,...
    Num_func,Min_Max_flag,Gl_Lo_flag]=initialize
N = 50;    % N is the number of the particles
K = 1000;  %K is the number of iteration
var = 5; % var is number of variables
L = 15 ; % L is the lenght for each variable
D = L*var; % D is the dimension of each particle
w_min=0.1;w_max=0.6;c1=2;c2=2;% w is the inertia factor and c1 & c2 are learning factors
position = rand(N,D)>0.5; % Generates initial population
fitness=0;
p_best = rand(N,D)>0.5;
g_best = rand(N,D)>0.5;
p_best_fit = ones(N,1);
Num_func = 1 ; % Select the number of function to be evaluated
Min_Max_flag = 1 ;  % 1 if the function must be minimized  ....  2 if the function must be maximized
Gl_Lo_flag = 1  ;   % 1 if the search is global ...............  2 if the search is local
return


% Version 1.000
%
% Code provided by Ruslan Salakhutdinov and Geoff Hinton
%
% Permission is granted for anyone to copy, use, modify, or distribute this
% program and accompanying programs and documents for any purpose, provided
% this copyright notice is retained and prominently displayed, along with
% a note saying that the original programs are available from our
% web page.
% The programs and documents are distributed without any warranty, express or
% implied.  As the programs were written for research purposes only, they have
% not been tested to the degree that would be advisable in any important
% application.  All use of these programs is entirely at the user's own risk.

% This program fine-tunes an autoencoder with backpropagation.
% Weights of the autoencoder are going to be saved in mnist_weights.mat
% and trainig and test reconstruction errors in mnist_error.mat
% You can also set maxepoch, default value is 200 as in our paper.

maxepoch=200;
fprintf(1,'\nFine-tuning deep autoencoder by minimizing cross entropy error. \n');
fprintf(1,'60 batches of 1000 cases each. \n');

load mnistvh
load mnisthp
load mnisthp2
load mnistpo

makebatches;
[numcases numdims numbatches]=size(batchdata);
N=numcases;

%%%% PREINITIALIZE WEIGHTS OF THE AUTOENCODER %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
w1=[vishid; hidrecbiases];
w2=[hidpen; penrecbiases];
w3=[hidpen2; penrecbiases2];
w4=[hidtop; toprecbiases];
w5=[hidtop'; topgenbiases];
w6=[hidpen2'; hidgenbiases2];
w7=[hidpen'; hidgenbiases];
w8=[vishid'; visbiases];

%%%%%%%%%% END OF PREINITIALIZATIO OF WEIGHTS  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

l1=size(w1,1)-1;
l2=size(w2,1)-1;
l3=size(w3,1)-1;
l4=size(w4,1)-1;
l5=size(w5,1)-1;
l6=size(w6,1)-1;
l7=size(w7,1)-1;
l8=size(w8,1)-1;
l9=l1;
test_err=[];
train_err=[];

for epoch = 1:maxepoch

%%%%%%%%%%%%%%%%%%%% COMPUTE TRAINING RECONSTRUCTION ERROR %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
err=0;
[numcases numdims numbatches]=size(batchdata);
N=numcases;
 for batch = 1:numbatches
  data = [batchdata(:,:,batch)];
  data = [data ones(N,1)];
  w1probs = 1./(1 + exp(-data*w1)); w1probs = [w1probs  ones(N,1)];
  w2probs = 1./(1 + exp(-w1probs*w2)); w2probs = [w2probs ones(N,1)];
  w3probs = 1./(1 + exp(-w2probs*w3)); w3probs = [w3probs  ones(N,1)];
  w4probs = w3probs*w4; w4probs = [w4probs  ones(N,1)];
  w5probs = 1./(1 + exp(-w4probs*w5)); w5probs = [w5probs  ones(N,1)];
  w6probs = 1./(1 + exp(-w5probs*w6)); w6probs = [w6probs  ones(N,1)];
  w7probs = 1./(1 + exp(-w6probs*w7)); w7probs = [w7probs  ones(N,1)];
  dataout = 1./(1 + exp(-w7probs*w8));
  err= err +  1/N*sum(sum( (data(:,1:end-1)-dataout).^2 ));
  end
 train_err(epoch)=err/numbatches;

%%%%%%%%%%%%%% END OF COMPUTING TRAINING RECONSTRUCTION ERROR %%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%% DISPLAY FIGURE TOP ROW REAL DATA BOTTOM ROW RECONSTRUCTIONS %%%%%%%%%%%%%%%%%%%%%%%%%
fprintf(1,'Displaying in figure 1: Top row - real data, Bottom row -- reconstructions \n');
output=[];
 for ii=1:15
  output = [output data(ii,1:end-1)' dataout(ii,:)'];
 end
   if epoch==1
   close all
   figure('Position',[100,600,1000,200]);
   else
   figure(1)
   end
   mnistdisp(output);
   drawnow;

%%%%%%%%%%%%%%%%%%%% COMPUTE TEST RECONSTRUCTION ERROR %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[testnumcases testnumdims testnumbatches]=size(testbatchdata);
N=testnumcases;
err=0;
for batch = 1:testnumbatches
  data = [testbatchdata(:,:,batch)];
  data = [data ones(N,1)];
  w1probs = 1./(1 + exp(-data*w1)); w1probs = [w1probs  ones(N,1)];
  w2probs = 1./(1 + exp(-w1probs*w2)); w2probs = [w2probs ones(N,1)];
  w3probs = 1./(1 + exp(-w2probs*w3)); w3probs = [w3probs  ones(N,1)];
  w4probs = w3probs*w4; w4probs = [w4probs  ones(N,1)];
  w5probs = 1./(1 + exp(-w4probs*w5)); w5probs = [w5probs  ones(N,1)];
  w6probs = 1./(1 + exp(-w5probs*w6)); w6probs = [w6probs  ones(N,1)];
  w7probs = 1./(1 + exp(-w6probs*w7)); w7probs = [w7probs  ones(N,1)];
  dataout = 1./(1 + exp(-w7probs*w8));
  err = err +  1/N*sum(sum( (data(:,1:end-1)-dataout).^2 ));
  end
 test_err(epoch)=err/testnumbatches;
 fprintf(1,'Before epoch %d Train squared error: %6.3f Test squared error: %6.3f \t \t \n',epoch,train_err(epoch),test_err(epoch));

%%%%%%%%%%%%%% END OF COMPUTING TEST RECONSTRUCTION ERROR %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 tt=0;
 for batch = 1:numbatches/10
 fprintf(1,'epoch %d batch %d\r',epoch,batch);

%%%%%%%%%%% COMBINE 10 MINIBATCHES INTO 1 LARGER MINIBATCH %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 tt=tt+1;
 data=[];
 for kk=1:10
  data=[data
        batchdata(:,:,(tt-1)*10+kk)];
 end

%%%%%%%%%%%%%%% PERFORM CONJUGATE GRADIENT WITH 3 LINESEARCHES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  max_iter=3;
  VV = [w1(:)' w2(:)' w3(:)' w4(:)' w5(:)' w6(:)' w7(:)' w8(:)']';
  Dim = [l1; l2; l3; l4; l5; l6; l7; l8; l9];

  [X, fX] = minimize(VV,'CG_MNIST',max_iter,Dim,data);

  w1 = reshape(X(1:(l1+1)*l2),l1+1,l2);
  xxx = (l1+1)*l2;
  w2 = reshape(X(xxx+1:xxx+(l2+1)*l3),l2+1,l3);
  xxx = xxx+(l2+1)*l3;
  w3 = reshape(X(xxx+1:xxx+(l3+1)*l4),l3+1,l4);
  xxx = xxx+(l3+1)*l4;
  w4 = reshape(X(xxx+1:xxx+(l4+1)*l5),l4+1,l5);
  xxx = xxx+(l4+1)*l5;
  w5 = reshape(X(xxx+1:xxx+(l5+1)*l6),l5+1,l6);
  xxx = xxx+(l5+1)*l6;
  w6 = reshape(X(xxx+1:xxx+(l6+1)*l7),l6+1,l7);
  xxx = xxx+(l6+1)*l7;
  w7 = reshape(X(xxx+1:xxx+(l7+1)*l8),l7+1,l8);
  xxx = xxx+(l7+1)*l8;
  w8 = reshape(X(xxx+1:xxx+(l8+1)*l9),l8+1,l9);

%%%%%%%%%%%%%%% END OF CONJUGATE GRADIENT WITH 3 LINESEARCHES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 end

 save mnist_weights w1 w2 w3 w4 w5 w6 w7 w8
 save mnist_error test_err train_err;

end


function [ signal_filter2 ] = medianfilter_is( signal, fs )
% [signal_filter2] = medianfilter_is( signal, fs )
%   OVERVIEW:   This function estimates the baseline wander signal in the ECG and returns the estimate in the signal_filter2 variable
%
%	INPUT: 	MANDATORY:
%               signal          : a single row of ECG data in samples.
%
%               fs              : sampling frequency for the ecg signal (Hz)
%
%
%   	OUTPUT:
%            	signal_filter2     : baseline wander estimate for the ecg in var signal.
%
%
%	REPO:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%   ORIGINAL SOURCE AND AUTHORS:
%       Written by Ismail Sadiq
%	COPYRIGHT (C) 2019
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information. The license may be found in the Documents
%       folder of the Physionet-Cardiovascular-Signal-Toolbox.

orderfilter1 = floor(0.2 * fs);
orderfilter2 = floor(0.6 * fs);

signal_filter1 = medfilt1(signal, orderfilter1);
signal_filter2 = medfilt1(signal_filter1, orderfilter2);

end


function c=mapcolor(d,map,clim)
% Usage: c=mapcolor(d,[map],[clim])
% Given an array "d", mapcolor uses the colormap "map" to assign a color to
% each entry in "d", returning the results as RGB triplets in "c". Provide
% a two-element vector "clim" to impose limits on the colormap. Basically
% this does exactly what Matlab's built-in colormaps do, but does not
% affect the "Colormap" property of the current figure--so you can use
% multiple colormaps on the same figure. See also mapcolorbar.m.
%
%   Example:
%      load clown
%      mylim=[min(X(:)) max(X(:))];
%      Xhot=mapcolor(X,hot,mylim);
%      subplot(211);
%      imagesc(Xhot);
%      mapcolorbar(hot,mylim);
%      Xjet=mapcolor(X,jet,mylim);
%      subplot(212);
%      imagesc(Xjet);
%      mapcolorbar(jet,mylim);

% Written 29 April 2010 by Douglas H. Kelley, dhk [at] dougandneely.com.

mapdefault=jet(256); % specifying a size prevents a figure from popping up

if nargin<1
    error(['Usage: c = ' mfilename '(d,[map],[clim])'])
end
if ~exist('map','var') || isempty(map)
    map=mapdefault;
elseif ischar(map)
    warning(['MATLAB:' mfilename ':mapIsString'], ...
        ['Map given as a string; attempting to evaluate ''' map '''.'])
    map=eval(map);
end
if ~exist('clim','var') || isempty(clim)
    clim=[min(d(:)) max(d(:))];
end

N=numel(d);
NN=size(d);
mapsize=size(map,1);
binnum=floor( mapsize/diff(clim)*(d-clim(1))+1 ); % A linear mapping...
binnum(isnan(binnum))=1; % ...setting NaNs to the mininum of the map, ...
binnum(binnum<1)=1; % ...then correcting for saturation at bottom...
binnum(binnum>mapsize)=mapsize; % ...and top.
if isvector(d)
    c=nan*ones(N,3);
    for ii=1:N
        c(ii,:)=map(binnum(ii),:);
    end
elseif ndims(d)==2
    c=nan*ones([size(d) 3]);
    [i,j]=ind2sub(NN,[1:N]);
    for ii=1:N
        c(i(ii),j(ii),:)=map(binnum(ii),:);
    end
else
    error(['Sorry, mapcolor does not support arrays of dimension '...
        'greater than two.'])
end


function [ranks, sortingInds, isTied] = prtUtilRank(ds)
% prtUtilRank - Ranks the vector in increasing magnitude
%   Ties have a rank of the middle of the tied ranks
%   NaNs have a rank of NaN
%
% prtUtilRank([1 2 2 4 5 inf nan nan]')

if ~isnumeric(ds)
    if isa(ds,'prtDataSetBase')
        ds = ds.getObservations();
    else
        error('prt:prtUtilRank','Input must be either a numerical vector or a prtDataSet containing 1 feature.')
    end
end

if isvector(ds)
    ds = ds(:);
end

assert(size(ds,2)==1,'prtUtilRank is only for 1 dimensional data');

[sortedDS, sortingInds] = sort(ds);

ranks = (1:length(sortedDS))';

if length(sortedDS) > 1
    isTiedWithNext = cat(1,sortedDS(1:(end-1)) == sortedDS(2:end),false);
else
    isTiedWithNext = false;
end

% If there are any ties we need to figure out the tied regions and set each
% of the ranks to the average of the tied ranks.
tieRegions = [];
if any(isTiedWithNext)
    diffIsTiedWithNext = diff(isTiedWithNext);

    if isTiedWithNext(1) % First one is tied
        diffIsTiedWithNext = cat(1,1,diffIsTiedWithNext);
    else
        diffIsTiedWithNext = cat(1,0,diffIsTiedWithNext);
    end

    % Start and stop regions of the ties
    tieRegions = cat(2,find(diffIsTiedWithNext==1),find(diffIsTiedWithNext==-1));

    for iRegion = 1:size(tieRegions,1);
        cInds = tieRegions(iRegion,1):tieRegions(iRegion,2);

        ranks(cInds) = mean(ranks(cInds));
    end
end

ranks(isnan(sortedDS)) = nan;

ranks(sortingInds) = ranks;

if nargout > 2
    % We asked for the isTied vector
    isTied = false(size(ds));
    for iRegion = 1:size(tieRegions,1);
        isTied(tieRegions(iRegion,1):tieRegions(iRegion,2)) = true;
    end
    isTied(sortingInds) = isTied;
end


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

% -*- texinfo -*-
% @deftypefn {Function File} {[@var{yout}] =} bohmanwin(@var{xin},@var{h}),@var{p},@var{q})
%	Upsample, filter and downsample a signal.
% @seealso{rectwin,  bartlett}
% @end deftypefn

function yout = upfirdn(xin,h,p,q)

if(nargin < 2)
  error('usage : yout = upfirdn(xin,h,p,q)');
end

if(nargin < 3)
	p = 1;
	q = 1;
end

if(nargin < 4)
	q = 1;
end

if(floor(p) ~= p || floor(q) ~= q || p < 1 || q < 1)
	error('p and q must be positive integer');
end

yout = upsample(xin,p);
yout = convn(yout, h).*p; % original was filter(h, 1, yout);
% the scaling with p is needed as per github issue 2085, causing the output
% to be scaled by the value of p, with this change, the compat/matlab
% versions will give an output that is about equal (scaled with about 0.9993)
yout = downsample(yout,q);


function triangulation_test27 ( )

%*****************************************************************************80
%
%% TEST27 tests TRIANGULATION_ORDER6_VERTEX_COUNT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 August 2006
%
%  Author:
%
%    John Burkardt
%
  dim_num = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST27\n' );
  fprintf ( 1, '  TRIANGULATION_ORDER6_VERTEX_COUNT counts the \n' );
  fprintf ( 1, '  vertex nodes and midside nodes in\n' );
  fprintf ( 1, '  an order 6 triangulation.\n' );

  [ node_num, triangle_num, hole_num ] = ...
    triangulation_order6_example1_size ( );

  [ node_xy, triangle_node, triangle_neighbor ] = ...
    triangulation_order6_example1 ( node_num, triangle_num );

  [ vertex_num, midside_num ] = triangulation_order6_vertex_count ( ...
    node_num, triangle_num, triangle_node );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of nodes         = %d\n', node_num );
  fprintf ( 1, '  Number of vertex nodes  = %d\n', vertex_num );
  fprintf ( 1, '  Number of midside nodes = %d\n', midside_num );

  return
end


function p=v_mos2pesq(m)
%V_MOS2PESQ convert MOS speech quality scores to PESQ p=(m)
%Inputs:    m  is a matrix of MOS scores
%
%Outputs:   p  is a matrix, the same size as m, of PESQ scores
%
% The PESQ measure is defined in [2]. The mapping function, defined in [3],
% converts raw PESQ scores (which lie in the range -0.5 to 4.5) onto the
% MOS-LQO (Mean Opinion Score - Listening Quality Objective [2]) scale in the
% range 1 to 5. The MOS scale is defined in [1] as
%           5=Excellent, 4=Good, 3=Fair, 2=Poor, 1=Bad.
%
% Refs: [1]	ITU-T. Methods for subjective determination of transmission quality.
%           Recommendation P.800, Aug. 1996.
%       [2]	ITU-T. Mean opinion score (MOS) terminology.
%           Recommendation P.800.1, July 2006.
%       [2]	ITU-T. Perceptual evaluation of speech quality (PESQ), an objective
%           method for end-to-end speech quality assessment of narrowband telephone
%           networks and speech codecs. Recommendation P.862, Feb. 2001.
%       [3]	ITU-T. Mapping function for transforming P.862 raw result scores to MOS-LQO.
%           Recommendation P.862.1, Nov. 2003.

%      Copyright (C) Mike Brookes 2012-2013
%      Version: $Id: v_mos2pesq.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=0.999;
    b=4.999-a;
    c=-1.4945;
    d=4.6607;
end
if nargout>0
    p=(log(b./(m-a)-1)-d)/c;
else
    if nargin<1 || isempty(m)
        pp=linspace(-0.5,4.5,100);
        mm=v_pesq2mos(pp);
    else
        mm=m;
    end
    p=v_mos2pesq(mm);
    plot(mm,p);
    ylabel('PESQ (P.862)');
    xlabel('Mean Opimion Score (MOS)');
end


function [x, infos] = snpa(V, num_col, in_options)
% Successive Nonnegative Projection Algorithm (variant with f(.) = ||.||^2)
%
%       At each step of the algorithm, the column of X maximizing ||.||_2 is
%       extracted, and X is updated with the residual of the projection of its
%       columns onto the convex hull of the columns extracted so far.
%
% Inputs:
%       matrix      V
%       num_col     number of columns to be extracted.
%       options     options
%           normalization: 1: scale the columns of X so that they sum to one,
%                           hence matrix H will satisfy the assumption above for any
%                           nonnegative separable matrix X.
%                          0: the default value for which no scaling is
%                           performed. For example, in hyperspectral imaging, this
%                           assumption is already satisfied and normalization is not
%                           necessary.
%
% Output:
%       w           solution of w
%           K        : index set of the extracted columns.
%           H        : optimal weights, that is, H = argmin_{Y >= 0} ||X-X(:,K)Y||_F
%       infos       information
%
% References:
%       N. Gillis,
%       "Successive Nonnegative Projection Algorithm for Robust Nonnegative Blind Source Separation,"
%       SIAM J. on Imaging Sciences 7 (2),
%       pp. 1420-1450, 2014.
%
%
% This file is part of NMFLibrary.
%
% This file has been ported from
%   SNPA.m at https://gitlab.com/ngillis/nmfbook/-/tree/master/algorithms
%   by Nicolas Gillis (nicolas.gillis@umons.ac.be)
%
% Change log:
%
%   June. 21, 2022 (Hiroyuki Kasai): Added initialization module.
%

    % set dimensions and samples
    [m, n] = size(V);

    % set local options
    local_options = [];
    local_options.disp_freq = 1;
    local_options.normalize = 0;
    local_options.relerr = 1e-6;
    local_options.inner_max_epoch = 500;
    local_options.inner_nnls_alg = 'fpgm';
    local_options.special_stop_condition = @(epoch, infos, options, stop_options) spna_stop_func(epoch, infos, options, stop_options);

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    % initialize
    method_name = 'SNPA';
    i = 0;
    grad_calc_count = 0;
    stop_options = [];

    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    % initialize for this algorithm
    options.max_epoch = num_col+1;
    if options.normalize == 1
        % normalize the columns of V of which colum is L1-norm = 1
        V = normalize_W(V, 1);
    end

    U = zeros(m, num_col);
    K = zeros(1, num_col);
    H = zeros(num_col, n);
    normV0 = sum(V.^2);
    nVmax = max(normV0);
    normR = normV0;
    VtUK = [];
    UKtUK = [];

    % set for nnls subsolver
    nnls_options = [];
    nnls_options.verbose = 0;
    nnls_options.inner_max_epoch = options.inner_max_epoch;
    nnls_options.algo = options.inner_nnls_alg;

    % store initial info
    clear infos;
    [infos, f_val, optgap] = store_nmf_info(V, eye(m), V, [], options, [], i, grad_calc_count, 0);

    if options.verbose > 1
        fprintf('%s: Epoch = 0000, cost = %.16e, optgap = %.4e\n', method_name, f_val, optgap);
    end

    % set start time
    start_time = tic();

    % main loop
    i = i + 1;
    while true

        % check stop condition
        stop_options.normR = normR;
        stop_options.nVmax = nVmax;
        [stop_flag, reason, max_reached_flag] = check_stop_condition(i, infos, options, stop_options);
        if stop_flag
            display_stop_reason(i, infos, options, method_name, reason, max_reached_flag);
            break;
        end

        % select the column of the residual R with largest l2-norm
        [a, ~] = max(normR);

        % check ties up to 1e-6 precision
        b = find((a-normR)/a <= 1e-6);

        % In case of a tie, select column with largest norm of the input matrix X
        if length(b) > 1
            [~, d] = max(normX0(b));
            b = b(d);
        end

        % update the index set, and extracted column
        K(i) = b;
        U(:, i) = V(:, b);

        % update MtUJ
        VtUK = [VtUK, V' * U(:, i)];

        % update UJtUJ
        if i == 1
            UtUi = [];
        else
            UtUi = U(:, 1:i-1)' * U(:, i);
        end
        UKtUK = [UKtUK, UtUi ; UtUi', U(:, i)' * U(:, i)];

        % update residual
        if i == 1
            % Fast gradient method for min_{y in Delta} ||M(:, i)-M(:,J)y||
            [H, ~, ~, ~] = nnls_solver(V, V(:, K(1:i)), nnls_options);
        else
            H(:, K(i)) = 0;
            h = zeros(1,n); h(K(i)) = 1;
            H = [H; h];
            nnls_options.init = H;
            [H, ~, ~, ~] = nnls_solver(V, V(:, K(1:i)), nnls_options);
        end

        % update the norm of the columns of the residual without computing it explicitely.
        if i == 1
            normR = normV0 - 2 * ((VtUK') .* H) + (H .* (UKtUK*H));
        else
            normR = normV0 - 2 * sum((VtUK') .* H) + sum(H .* (UKtUK*H));
        end

        % measure elapsed time
        elapsed_time = toc(start_time);

        % measure gradient calc count
        grad_calc_count = grad_calc_count + m*n;

        % update epoch
        i = i + 1;

        % store info
        infos = store_nmf_info(V, U(:, 1:i-1), H, [], options, infos, i-1, grad_calc_count, elapsed_time);

        % display info
        display_info(method_name, i-1, infos, options);

    end

    x.K = K;
    x.U = U;
    x.H = H;

end

function [stop_flag, reason, rev_infos] = spna_stop_func(epoch, infos, options, stop_options)

    stop_flag = false;
    reason = [];
    rev_infos = [];

    normR = stop_options.normR;
    nVmax = stop_options.nVmax;

    if sqrt(max(normR)/nVmax) < options.relerr
        stop_flag = true;
        reason = sprintf('precision reached: sqrt(max(normR)/nVmax) = %.4e < options.relerr = %.4e\n', sqrt(max(normR)/nVmax), options.relerr);
        return;
    end

end


function V_RF  = yuweiA1

global Vn H Nrf Nt Nk
V_RF = ones(Nt,Nrf);

for k = 1:Nk
    F(:,:,k) = H(:,:,k)'*H(:,:,k);
end
F = sum(F,3)/Nk;
g = 1/Nrf/Nt;
a = g/Vn;

for Nloop = 1:10
    for j = 1:Nrf
        VRF = V_RF;
        VRF(:,j)=[];
        C = eye(Nrf-1)+a*VRF'*F*VRF;
        G = a*F-a^2*F*VRF*C^(-1)*VRF'*F;
        for i = 1:Nt
            for l = 1:Nt
                if i~=l
                    x(l)=G(i,l)*V_RF(l,j);
                end
            end
            n = sum(x);
            if n ==0
                V_RF(i,j)=1;
            else
                V_RF(i,j)=n/abs(n);
            end
        end
    end
end


%ROC Receiver-Operator Curve
%
%   E = ROC(B,C,N)
%
% INPUT
%   A  Dataset
%   W  Trained classifier, or
%   B  Classification result, B = A*W*CLASSC
%   C  Index of desired class (default: C = 1)
%   N  Number of points on the Receiver-Operator Curve (default: 100)
%
% OUTPUT
%   E  Structure containing the error of the two classes
%
% DESCRIPTION
% Computes N points on the receiver-operator curve of the classifier W for
% class C in the labeled dataset B, which is typically the result of
% B = A*W; or for the dataset A labelled by applying the (cell array of)
% trained classifiers W.
%
% Note that a Receiver-Operator Curve is related to a specific class (class C)
% for which the errors are plotted horizontally. The total error on all other
% classes is plotted vertically. The class index C refers to its position in
% the label list of the dataset (A or B). It can be found by GETCLASSI.
%
% The curve is computed for N thresholds of the posteriori probabilities
% stored in B. The resulting error frequencies for the two classes are
% stored in the structure E. E.XVALUES contains the errors in the first
% class, E.ERROR contains the errors in the second class. In multi-class
% problems these are the mean values in a single class, respectively the
% mean values in all other classes. This may not be very useful, but not
% much more can be done as for multi-class cases the ROC is equivalent to a
% multi-dimensional surface.
%
% Use PLOTE(E) for plotting the result. In the plot the two types of error
% are annotated as 'Error I' (error of the first kind) and 'Error II' (error
% of the second kind). All error estimates are weighted according the class
% prior probabilities. Remove the priors in A or B (by setprior(A,[])) to
% produce a vanilla ROC.
%
% EXAMPLES
%	Train set A and test set T:
%	  B = T*NMC(A); E = ROC(T,50); PLOTE(E); % Plots a single curve
%	  E = ROC(T,A*{NMC,UDC,QDC});  PLOTE(E); % Plots 3 curves
%

function [roc, thr] = roc_detection(a,n)

	% Depending on the call, CLAS may the third or second argument.
	% and N the third or the fourth.

	if nargin < 2 || isempty(n), n = 100; end

	datname = getname(a);
	lablist = getlablist(a,'string');

    required_labs = {'FP' 'TP'};
    aux_lablist = intersect(cellstr(lablist), required_labs );

    cant_required_labs = length(required_labs);

    if( length(aux_lablist) ~= cant_required_labs )
        fprintf(2, ['Esta funcion esta pensada para usarse en datasets de deteccion con labels:\n' colvec([char(required_labs) repmat('\n', cant_required_labs, 1)]')' ] );
        error();
    end

    clas = find( strcmpi(cellstr(lablist), 'TP') );
	clasname = lablist(clas,:);
	%DXD: also check the class sizes:
	cs = classsizes(a);
	if any(cs == 0)
		error('Ambas clases deben contener ejemplos');
	end

	% Set up the ROC structure.

    % If a cell array of classifiers was given, apply each one here.

    a = a*normm; % make sure we have a normalised classification matrix

    [m,c] = size(a);
    nlab = getnlab(a);
    d = sort(a(:));

    % Attempt to compute a good threshold range on the first classified
    % dataset.
%     thr = [max(0, min(d)-eps) rowvec(d(round(linspace(2,length(d)-1, n-1)))) 1];
    thr = linspace(0,1,n+1);

    % NLAB_OUT will be one where B is larger than THR.
    I = matchlablist(getlablist(a),getfeatlab(a)); % Correct possible changes class orders
    nlab_out = (repmat(+a(:,I(clas)),1,n+1) >= repmat(thr,m,1));

    % aciertos will be 1 where the numeric label is unequal to NLAB_OUT
    % (i.e., where errors occur).
    bTP = nlab == clas;
    aciertos = (repmat(bTP,1,n+1) == nlab_out);

    sensitivity = mean(aciertos(bTP,:),1); % S
    pospred = sum(aciertos(bTP,:)) ./ sum(nlab_out); % +P

    roc = [ colvec(sensitivity) colvec(pospred) ];

    mod = sqrt(sum(roc.^2,2));
    [max_mod max_mod_idx] = max(mod);

    figure(100);
    h = subplot(1,2,1);
    plot(h(1), sensitivity(:), pospred(:), 'bo-' )
    hold(h(1), 'on');
    plot(h(1), sensitivity(max_mod_idx), pospred(max_mod_idx), 'rx:', 'MarkerSize',11)
    plot(h(1), sensitivity(max_mod_idx), pospred(max_mod_idx), 'mo:', 'MarkerSize',11)
    hold(h(1), 'off');
    axis(h(1), 'square');
    box(h(1), 'off')
    xlabel(h(1), 'Sensitivity')
    ylabel(h(1), 'Positive predictivity')
%     x_lim = xlim();
%     y_lim = ylim();
%     xlim([min([x_lim ylim]) 1.1]);
%     xlim([min([x_lim ylim]) 1.1]);

    h(2) = subplot(1,2,2);
    plot(h(2), thr, mod, 'bo-' )
    hold(h(2), 'on');
    plot(h(2), thr(max_mod_idx), mod(max_mod_idx), 'rx', 'MarkerSize',11)
    plot(h(2), thr(max_mod_idx), mod(max_mod_idx), 'mo', 'MarkerSize',11)
    plot(h(2), [0.5 0.5], ylim(), 'k--')
    hold(h(2), 'off');
    box(h(2), 'off')
    ylabel(h(2), 'mod(S,P+)')
    xlabel(h(2), 'Operating point')

    ConfusionMat = [ sum(aciertos(~bTP,max_mod_idx))  sum(~aciertos(~bTP,max_mod_idx)); sum(~aciertos(bTP,max_mod_idx)) sum(aciertos(bTP,max_mod_idx)) ];

    DisplayResults('dsResult', ConfusionMat, 'SupportDataset', a);


function [ x, seed ] = square01_sample ( n, seed )

%*****************************************************************************80
%
%% SQUARE01_SAMPLE samples points in the unit square in 2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 January 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Output, real X(2,N), the points.
%
  m = 2;

  [ x, seed ] = r8mat_uniform_01 ( m, n, seed );

  return
end


function [D, matches] = run_icp_fixed(M, N, C_init, max_iters)

verbose = 1;

if verbose
    fprintf('Running ICP...\n');
end

ann_params = struct;
% ann_params.algorithm = 'linear'; %use 'kmeans' for some speedup
% ann_params.trees = 8;
% ann_params.checks = 64;
% ann_params.centers_init = 'kmeanspp';
% ann_params.iterations = -1;

kN = size(N.evecs,1);

% flann_search(target, query)
[matches, dists] = flann_search(C_init*M.evecs', N.evecs', 1, ann_params);
% [matches, dists] = knnsearch(N.S*N.evecs, M.S*M.evecs*C_init');

err = sum(sqrt(dists));
err = err / (kN*size(C_init,1));

if verbose
    fprintf('(0) MSE: %.2e\n', err);
end

if max_iters == 0
    D = C_init;
%     matches = matches';
    return
end

% Start iterations

D_prev = C_init;
err_prev = err;
matches_prev = matches;

% vidx = 2:4;
% figure, plot_cloud([],N.evecs(:,vidx),'b.'); axis equal; hold on; plot_cloud([],M.evecs(:,vidx),'r.');
% figure, plot_cloud([],N.evecs(:,vidx),'b.'); axis equal; hold on; pp = M.evecs*C_init';plot_cloud([],pp(:,vidx),'r.');

% [u,~,v] = svd(C_init);
% D = u*v';

for i=1:max_iters

%     if i>1
        [U,~,V] = svd((M.evecs(matches,:)'*M.S(matches,matches)) * (N.S*N.evecs));
        D = U * V(:,1:size(C_init,2))';
        D = D';
%     end

%     figure, plot_cloud([],N.evecs(:,vidx),'b.'); axis equal; hold on; pp = M.evecs*D';plot_cloud([],pp(:,vidx),'r.');

    %     matches = flann_search(M.evecs', D*N.evecs', 1, ann_params);
    [matches, dists] = flann_search(D*M.evecs', N.evecs', 1, ann_params);
%     [matches, dists] = knnsearch(N.S*N.evecs, M.S*M.evecs*D');
    err = sum(sqrt(dists));
    err = err / (kN*size(C_init,1));

    if verbose
        fprintf('(%d) MSE: %.2e\n', i, err);
    end

    if err > err_prev
        if verbose
            fprintf('Local optimum reached.\n');
        end
        D = D_prev;
        matches = matches_prev;
        break;
    end

    if (err_prev - err) < 5e-6
        if verbose
            fprintf('Local optimum reached.\n');
        end
        break;
    end

    err_prev = err;
    D_prev = D;
    matches_prev = matches;

end

% matches = matches';

end


% SCRIPT TEST FOR THE KUKA LBR ROBOT KINEMATICS

% 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/>.
robot = load_robot('KUKA', 'LBR_IIWA_R820_COP');
close all

% test both solutions: based on the transpose an on the Moore-Penrose
q = pi/8*[1,1,1,1,1,1,1]';
q = [0.1 0.2 .3 .4 .5 .6 .7];
drawrobot3d(robot, q)
J = manipulator_jacobian(robot, q);

% rigth
R = J'*inv(J*J');

% left
L=inv(J'*J)*J';

M = pinv(J)

R-L


function linplus_test50 ( )

%*****************************************************************************80
%
%% TEST50 tests R8PO_FA, R8PO_SL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  n = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST50\n' );
  fprintf ( 1, '  R8PO_FA factors a positive definite symmetric\n' );
  fprintf ( 1, '    linear system,\n' );
  fprintf ( 1, '  R8PO_SL solves a factored system.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N = %d\n', n );

  for i = 1 : n
    for j = 1 : n
      a(i,j) = min ( i, j );
    end
  end
%
%  Set the desired solution.
%
  x = r8vec_indicator ( n );
%
%  Compute the corresponding right hand side.
%
  b = r8po_mxv ( n, a, x );
%
%  Factor the matrix.
%
  [ a_lu, info ] = r8po_fa ( n, a );

  if ( info ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Fatal error!\n' );
    fprintf ( 1, '  R8PO_FA declares the matrix is singular!\n' );
    fprintf ( 1, '  The value of INFO is %d\n', info );
    return
  end
%
%  Solve the linear system.
%
  x = r8po_sl ( n, a_lu, b );

  r8vec_print ( n, x, '  Solution:' );
%
%  Set the desired solution.
%
  x(1:n) = 1;
%
%  Compute the corresponding right hand side, using the factored matrix.
%
  b = r8po_ml ( n, a, x );
%
%  Solve the linear system.
%
  x = r8po_sl ( n, a, b );

  r8vec_print ( n, x, '  Solution:' );

  return
end


%% Finding contours in an image
% We learn how to find contours of objects in our image.
%
% In this sample you will learn how to use the following OpenCV functions:
%
% * <matlab:doc('cv.findContours') cv.findContours>
% * <matlab:doc('cv.drawContours') cv.drawContours>
%
% Sources:
%
% * <https://docs.opencv.org/3.1.0/df/d0d/tutorial_find_contours.html>
% * <https://github.com/opencv/opencv/blob/3.1.0/samples/cpp/tutorial_code/ShapeDescriptors/findContours_demo.cpp>
%

function varargout = findContours_demo_gui(im)
    % load source image
    if nargin < 1
        src = imread(fullfile(mexopencv.root(),'test','HappyFish.jpg'));
    elseif ischar(im)
        src = imread(im);
    else
        src = im;
    end

    % Convert image to gray and blur it
    if size(src,3) == 3
        src = cv.cvtColor(src, 'RGB2GRAY');
    end
    src = cv.blur(src, 'KSize',[3 3]);

    % create the UI
    h = buildGUI(src);
    if nargout > 0, varargout{1} = h; end
end

function onChange(~,~,h)
    %ONCHANGE  Event handler for UI controls

    % retrieve current values from UI controls
    thresh = round(get(h.slid, 'Value'));
    set(h.txt, 'String',sprintf('Canny thresh: %3d',thresh));

    % Detect edges using canny
    canny_output = cv.Canny(h.src, [thresh thresh*2], 'ApertureSize',3);

    % Find contours
    [contours, hierarchy] = cv.findContours(canny_output, ...
        'Mode','Tree', 'Method','Simple');

    % Draw contours
    drawing = zeros([size(canny_output) 3], 'uint8');
    for i=1:numel(contours)
        clr = randi([0 255], [1 3], 'uint8');
        drawing = cv.drawContours(drawing, contours, ...
            'Hierarchy',hierarchy, 'ContourIdx',i-1, 'MaxLevel',0, ...
            'Color',clr, 'Thickness',2, 'LineType',8);
    end

    % show result
    set(h.img, 'CData',drawing);
    drawnow;
end

function h = buildGUI(img)
    %BUILDGUI  Creates the UI

    % parameters
    thresh = 100;
    max_thresh = 255;
    sz = size(img);
    sz(2) = max(sz(2), 250);  % minimum figure width

    % build the user interface (no resizing to keep it simple)
    h = struct();
    h.src = img;
    h.fig = figure('Name','Contours Demo', ...
        'NumberTitle','off', 'Menubar','none', 'Resize','off', ...
        'Position',[200 200 sz(2) sz(1)+29]);
    if ~mexopencv.isOctave()
        %HACK: not implemented in Octave
        movegui(h.fig, 'center');
    end
    h.ax = axes('Parent',h.fig, 'Units','pixels', 'Position',[1 30 sz(2) sz(1)]);
    if ~mexopencv.isOctave()
        h.img = imshow(img, 'Parent',h.ax);
    else
        %HACK: https://savannah.gnu.org/bugs/index.php?45473
        axes(h.ax);
        h.img = imshow(img);
    end
    h.txt = uicontrol('Parent',h.fig, 'Style','text', 'FontSize',11, ...
        'Position',[5 5 130 20], 'String',sprintf('Canny thresh: %3d',thresh));
    h.slid = uicontrol('Parent',h.fig, 'Style','slider', 'Value',thresh, ...
        'Min',0, 'Max',max_thresh, 'SliderStep',[1 10]./(max_thresh-0), ...
        'Position',[135 5 sz(2)-135-5 20]);

    % hook event handlers, and trigger default start
    set(h.slid, 'Callback',{@onChange,h}, ...
        'Interruptible','off', 'BusyAction','cancel');
    onChange([],[],h);
end


function days = ymd_dif_common ( y1, m1, d1, y2, m2, d2 )

%*****************************************************************************80
%
%% YMD_DIF_COMMON gets the day difference between two Common YMD dates.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 October 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y1, M1, D1, the first YMD date.
%
%    Input, integer Y2, M2, D2, the second YMD date.
%
%    Output, integer DAYS, the number of days between the dates.
%
  days = 0;
%
%  Check the dates.
%
  [ y1, m1, d1, ierror ] = ymd_check_common ( y1, m1, d1 );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'YMD_DIF_COMMON - Fatal error!\n' );
    fprintf ( 1, '  Y1/M1/D1 is illegal.\n' );
    error ( 'YMD_DIF_COMMON - Fatal error!' );
  end

  [ y2, m2, d2, ierror ] = ymd_check_common ( y2, m2, d2 );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'YMD_DIF_COMMON - Fatal error!\n' );
    fprintf ( 1, '  Y2/M2/D2 is illegal.\n' );
    error ( 'YMD_DIF_COMMON - Fatal error!' );
  end

  jed1 = ymd_to_jed_common ( y1, m1, d1 );

  jed2 = ymd_to_jed_common ( y2, m2, d2 );

  days = round ( jed2 - jed1 );

  return
end


function [err, g] = metric_logssd(vol_fix, voldef, dvol)
    vol_diff = voldef - vol_fix;
    vdsq = vol_diff.^2;
    err = log(1 + sum(sum(sum(vdsq)))) * dvol;
    g = 2 * vol_diff ./ (1 + vdsq) * dvol;
end


function [disp_row, disp_col, sind] = resp_newton(response, responsef, iterations, ky, kx, use_sz)

[max_resp_row, max_row] = max(response, [], 1);
[init_max_response, max_col] = max(max_resp_row, [], 2);
max_row_perm = permute(max_row, [2 3 1]);
col = max_col(:)';
row = max_row_perm(sub2ind(size(max_row_perm), col, 1:size(response,3)));

trans_row = mod(row - 1 + floor((use_sz(1)-1)/2), use_sz(1)) - floor((use_sz(1)-1)/2);
trans_col = mod(col - 1 + floor((use_sz(2)-1)/2), use_sz(2)) - floor((use_sz(2)-1)/2);
init_pos_y = permute(2*pi * trans_row / use_sz(1), [1 3 2]);
init_pos_x = permute(2*pi * trans_col / use_sz(2), [1 3 2]);
max_pos_y = init_pos_y;
max_pos_x = init_pos_x;

% pre-compute complex exponential
exp_iky = exp(bsxfun(@times, 1i * ky, max_pos_y));
exp_ikx = exp(bsxfun(@times, 1i * kx, max_pos_x));

% gradient_step_size = gradient_step_size / prod(use_sz);

ky2 = ky.*ky;
kx2 = kx.*kx;

iter = 1;
while iter <= iterations
    % Compute gradient
    ky_exp_ky = bsxfun(@times, ky, exp_iky);
    kx_exp_kx = bsxfun(@times, kx, exp_ikx);
    y_resp = mtimesx(exp_iky, responsef, 'speed');
    resp_x = mtimesx(responsef, exp_ikx, 'speed');
    grad_y = -imag(mtimesx(ky_exp_ky, resp_x, 'speed'));
    grad_x = -imag(mtimesx(y_resp, kx_exp_kx, 'speed'));
    ival = 1i * mtimesx(exp_iky, resp_x, 'speed');
    H_yy = real(-mtimesx(bsxfun(@times, ky2, exp_iky), resp_x, 'speed') + ival);
    H_xx = real(-mtimesx(y_resp, bsxfun(@times, kx2, exp_ikx), 'speed') + ival);
    H_xy = real(-mtimesx(ky_exp_ky, mtimesx(responsef, kx_exp_kx, 'speed'), 'speed'));
    det_H = H_yy .* H_xx - H_xy .* H_xy;

    % Compute new position using newtons method
    max_pos_y = max_pos_y - (H_xx .* grad_y - H_xy .* grad_x) ./ det_H;
    max_pos_x = max_pos_x - (H_yy .* grad_x - H_xy .* grad_y) ./ det_H;

    % Evaluate maximum
    exp_iky = exp(bsxfun(@times, 1i * ky, max_pos_y));
    exp_ikx = exp(bsxfun(@times, 1i * kx, max_pos_x));

    iter = iter + 1;
end
max_response = 1 / prod(use_sz) * real(mtimesx(mtimesx(exp_iky, responsef, 'speed'), exp_ikx, 'speed'));

% check for scales that have not increased in score
ind = max_response < init_max_response;
max_response(ind) = init_max_response(ind);
max_pos_y(ind) = init_pos_y(ind);
max_pos_x(ind) = init_pos_x(ind);

[max_scale_response, sind] = max(max_response(:));
disp_row = (mod(max_pos_y(1,1,sind) + pi, 2*pi) - pi) / (2*pi) * use_sz(1);
disp_col = (mod(max_pos_x(1,1,sind) + pi, 2*pi) - pi) / (2*pi) * use_sz(2);
end


function Xsol=Bouguet2Devernay()
%
% J. Huai 19 jul 2014
clear all
close all
load H:\relaylatest\toolbox_calib\calib_casios\Calib_Results.mat
addpath H:\relaylatest\EKF_monoSLAM_1pRANSAC\matlab_code\matlabcalibration2ourcalibration\TOOLBOX_calib;
% for the test case of casio 2

cam.nRows = 720;
cam.nCols = 1280;
cam.fx= fc(1);
cam.fy= fc(2);
cam.cx= cc(1);
cam.cy= cc(2);
Xini =0.99; %-kc(1)*5;

% Image size
nCols = cam.nCols;
nRows = cam.nRows;

% Step for the image grid
inc = round(min(nCols,nRows)/30);
% Image grid
[u_grid,v_grid] = meshgrid(3*inc:inc:nCols-3*inc,3*inc:inc:nRows-3*inc);
[nGrid,mGrid] = size(u_grid);
uv_d = [reshape(u_grid,1, nGrid*mGrid); reshape(v_grid,1,nGrid*mGrid)];

xy_u = normalize(uv_d,fc,cc,kc,alpha_c);

% Non-linear minimization, minimizes the error in the image grid between the
% Bouguet's calibration model and the one we use in our code. Xsol should
% return the calibration parameters for our code in the following order:
% omega, fx, fy, cx, cy.
Xsol = lsqnonlin('matlab2Devernay_error',...
    Xini,0,1,...
    optimset('LargeScale','on','Display','Iter','TolFun',1e-9),cam, uv_d,xy_u)