%{
This file shows an example of using the full power of TFOCS. If you
are not familiar with basic usage of TFOCS, please see other demos
first.

We make an example that uses:
    -several variables (2), and the variables are matrices, not just vectors
    -several constraints (4)
    -affine operators (linear plus offset), and two of the operators
        are matrix --> matrix operators
    -debug mode

For small complicated examples like this, TFOCS is not necessarily
faster than software like CVX, since it takes more iterations than an
interior point method and there is some overhead in the TFOCS software
since the software is meant for flexibility rather than absolute speed.

However, if you take any complicated problem and scale the size by a factor
of 100, then CVX won't be able to handle it at all. TFOCS will do
just fine (and it might even be less than 100x as slow, since now the overhead
is not significant).

The problem we will solve:

min_{X1, X2} smooth1(X1)+smooth2(X2) +  sum_{i=1}^4  g_i( A1_i*X1 + A2_i*X2 + B_i )

meaning that we have 2 variables (X1 and X2), both of which are matrices,
and each variable has its own smooth function.
For non-smooth and/or constraint terms (indexed by "i"), we have 4 functions
( i = 1,2,3,4) g_i, and each has it's own affine operator in X1 and X2.
So each of the 4 affine operators has three parts: the portion linear in X1,
the portion linear in X2, and the constant offset "B_i".

To be explicit, the smooth functions are linear and quadratic, resp.:

smooth1(X1) = dot( s1, X1 ) + 3.4                  ("s1" is a matrix the same size as X1)
smooth2(X2) = dot( X2, X2 ) + dot( s2, X2 ) + 4.5  ("s2" is a matrix the same size as X2)

and the non-smooth/constraint terms are:

g_1(z)  = indicator set of the positive orthant of R^10
g_2(z)  = ||z||_2 (usual Euclidean norm) in R^15
g_3(z)  = ||z||_1 (l1 norm) in R^{10 x 20 }. This views a 10 x 20 matrix as a 200 x 1 vector.
g_4(z)  = indicator set of positive orthant of R^{20 x 22 }, i.e. each element must be >= 0 coordinate-wise

The affine operators are picked arbitrary, but some of them (#3 and #4) have a range that
is a set of matrices, rather than a set of vectors, in order to make
this more interesting.

%}

fileName = fullfile('reference_solutions','complicatedProblem1');
randn('state',29324);
rand('state',9332);
% rng(3481); % this only works on new versions of Matlab

% -- Variables --
% Two sets of variables, X1 (a matrix of size n1 x n2 ) and X2 (matrix of N1 x N2 )
n1 = 10; n2 = 20;
N1 = 12; N2 = 18;
X1 = zeros( n1, n2 );
X2 = zeros( N1, N2 );

% -- the smooth terms --
%   Note: the inner product used is the matrix inner product
%       that induces the Frobenius norm.
s1      = randn(n1,n2);
smooth1 = smooth_linear( s1, 3.4 );
s2      = randn(N1,N2);
smooth2 = smooth_quad( 1, s2, 4.5 );
% and the same thing, but in a format that CVX likes:
smoothF = @(X1,X2) vec(X1)'*vec(s1) + 3.4 + vec(X2)'*vec(s2) + 4.5 + ...
    sum_square(vec(X2))/2;

% -- some proximal terms --

nProx   = 4;
prox1   = proj_Rplus;
prox2   = proj_l2;          % primal is norm( , 2)
prox3   = proj_linf(1);     % this can take matrix varaibles...
prox4   = proj_Rplus;       % this can take matrix variables...
% Sizes of proxes (i.e. sizes of dual variables, i.e. size of range of linear terms)
d1      = [ 10, 1  ];
d2      = [ 15, 1  ];
d3      = [ n1, n2 ];
d4      = [ 20, 22 ];

% -- and linear terms --

% for prox1:
const1  = randn(d1);
temp1   = randn( prod(d1), n1*n2);
A1_X1   = linop_compose( linop_matrix(temp1), linop_vec([n1,n2]) );
temp2   = randn( prod(d1), N1*N2);
A1_X2   = linop_compose( linop_matrix(temp2), linop_vec([N1,N2]) );

A1      = @(X1,X2) temp1*vec(X1) + temp2*vec(X2) + const1;

% for prox2: (matrix variable)
const2  = randn(d2);
temp1   = randn( prod(d2), n1*n2);
A2_X1   = linop_compose( linop_matrix(temp1), linop_vec([n1,n2]) );
temp2   = randn( prod(d2), N1*N2);
A2_X2   = linop_compose( linop_matrix(temp2), linop_vec([N1,N2]) );

A2      = @(X1,X2) temp1*vec(X1) + temp2*vec(X2) + const2;

% for prox3:
const3  = 0;
A3_X1   = 63.4;     % this represents abstract scaling, i.e. any size input
A3_X2   = 0;        % this reprsents the zero linear operator

A3      = @(X1,X2) A3_X1*X1;

% for prox4: (matrix variable)
const4  = randn(d4);
temp1   = randn( prod(d4), n1*n2);
A4_X1   = linop_compose( linop_matrix(temp1), linop_vec([n1,n2]) );
rs      = linop_adjoint( linop_vec(d4) );
A4_X1   = linop_compose( rs, A4_X1 );
mat1    = @(x) reshape( x, d4(1), d4(2) );

temp2   = randn( prod(d4), N1*N2);
A4_X2   = linop_compose( linop_matrix(temp2), linop_vec([N1,N2]) );
rs      = linop_adjoint( linop_vec(d4) );
A4_X2   = linop_compose( rs, A4_X2 );

A4      = @(X1,X2) mat1( temp1*vec(X1) + temp2*vec(X2) ) + const4;

% -- set the smoothing parameter --
mu = 1;

if exist([fileName,'.mat'],'file')
    load(fileName); % contains X_CVX
    fprintf('Loaded problem from %s\n', fileName );
else
    % Get reference solution in CVX
    % First, get a solution to the smoothed version:
    cvx_begin
        variables X1(n1,n2) X2(N1,N2)
        % it's important that we use norm(vec(...),1) and not just norm(...,1),
        %   otherwise CVX interprets this with the wrong implicit inner product
        minimize(     smoothF( X1, X2 ) + ...
            norm( A2(X1,X2), 2 ) + norm( vec(A3(X1,X2)) , 1 ) + ...
            mu*( sum_square(vec(X1)) + sum_square(vec(X2)) )/2       )
        subject to
            A1(X1,X2) >= 0  % constraint is dual of prox1
            A4(X1,X2) >= 0  % constraint is dual of prox2
    cvx_end
    X_CVX_smoothed{1} = X1;
    X_CVX_smoothed{2} = X2;

    % Second, get a solution to the unsmoothed version
    cvx_begin
        variables X1(n1,n2) X2(N1,N2)
        minimize(     smoothF( X1, X2 ) + ...
            norm( A2(X1,X2), 2 ) + norm( vec(A3(X1,X2)) , 1 ) )
        subject to
            A1(X1,X2) >= 0
            A4(X1,X2) >= 0
    cvx_end
    X_CVX{1} = X1;
    X_CVX{2} = X2;

    save(fileName,'X_CVX', 'X_CVX_smoothed');
    fprintf('Saved data to file %s\n', fileName);
end

% Verify constraint are satisfied
fprintf('Is A1(x1,x2) >= 0? The min element is %g\n', min(    A1(X_CVX{1},X_CVX{2} )) )
fprintf('Is A2(x1,x2) >= 0? The min element is %g\n', min(min(A4(X_CVX{1},X_CVX{2}))) )
%% before running TFOCS, scale the problem perhaps?
% This is one way to see how big the norms are:
% nrm11    = linop_test( A1_X1 ); % 15.7
% nrm12    = linop_test( A1_X2 ); % 16.6
%
% nrm21    = linop_test( A2_X1 ); % 17.8
% nrm22    = linop_test( A2_X2 ); % 18.6
%
% nrm31    = linop_test( A3_X1 ); % 63.4
% nrm32    = linop_test( A3_X2 ); % 0
%
% nrm41    = linop_test( A4_X1 ); % 35.6
% nrm42    = linop_test( A4_X2 ); % 34.6

%% now, run TFOCS. First, try it without continuation

x0 = [];
z0 = [];
opts    = struct('continuation',false,'maxits',1500,'debug',true); % using 'debug' mode to print out useful information
opts.printEvery     = 50;

% Pick a scaling factor "s" (optional: set to "1" to have no effect)
s = .5; % helps a little bit
% (note that if we scale prox3 by "s", then we multiply the corresponding
%  affine part by "s", rather than divide them by "s", since prox3
%  is really for the dual and not the primal).

mu  = 1;
opts.errFcn{1} = @(f,d,p) norm( p{1}-X_CVX_smoothed{1}); % compare to smoothed reference solution
opts.errFcn{2} = @(f,d,p) norm( p{2}-X_CVX_smoothed{2});

[xAll,outParam,optsOut] = tfocs_SCD( {smooth1,smooth2}, ...
    { A1_X1, A1_X2, const1; A2_X1, A2_X2, const2; ...
    A3_X1*s, A3_X2*s, const3*s; A4_X1, A4_X2, const4 }, ...
    {prox1,prox2,prox_scale(prox3,s),prox4},...
    mu, x0, z0, opts );

mnConstraint1    = min(min( A1(xAll{1},xAll{2} ) ) );
mnConstraint2    = min(min( A4(xAll{1},xAll{2} ) ) );
fprintf('First constraint violated by:   %g\n', mnConstraint1);
fprintf('Second constraint violated by:  %g\n', mnConstraint2 );

% Check that we are within acceptable limits
er = sqrt(norm( xAll{1} - X_CVX_smoothed{1} )^2 + norm( xAll{2} - X_CVX_smoothed{2} )^2 )/...
    sqrt( norm(X_CVX_smoothed{1})^2 + norm(X_CVX_smoothed{2})^2 ); % should be about .006

if er > 0.04 || mnConstraint1 < -.01 || mnConstraint2 < -.01
    error('Failed the test');
else
    disp('This test successfully passed');
end

%% run TFOCS with continuation
x0 = [];
z0 = [];
% type "tfocs" at the command to see possible options
opts    = struct('continuation',true,'maxits',1500);
opts.printEvery     = 50;
opts.tol            = 1e-4;
opts.stopCrit       = 4;
opts.printStopCrit  = true;

% type "continuation" at the command to see possible options
contOpts    = struct( 'maxIts', 8 , 'muDecrement', 0.8 );
% ask for increased accuracy on the final solve
contOpts.finalTol = 1e-5;

s = .5; % helps a little bit
% (note that if we scale prox3 by "s", then we multiply the corresponding
%  affine part by "s", rather than divide them by "s", since prox3
%  is really for the dual and not the primal).

mu  = 10;
opts.errFcn{1} = @(f,d,p) norm( p{1}-X_CVX{1}); % compare to unsmoothed reference solution
opts.errFcn{2} = @(f,d,p) norm( p{2}-X_CVX{2});

[xAll,outParam,optsOut] = tfocs_SCD( {smooth1,smooth2}, ...
    { A1_X1, A1_X2, const1; A2_X1, A2_X2, const2; ...
    A3_X1*s, A3_X2*s, const3*s; A4_X1, A4_X2, const4 }, ...
    {prox1,prox2,prox_scale(prox3,s),prox4},...
    mu, x0, z0, opts, contOpts );

mnConstraint1    = min(min( A1(xAll{1},xAll{2} ) ) );
mnConstraint2    = min(min( A4(xAll{1},xAll{2} ) ) );
fprintf('First constraint violated by:   %g\n', mnConstraint1);
fprintf('Second constraint violated by:  %g\n', mnConstraint2 );

% Check that we are within acceptable limits
er = sqrt(norm( xAll{1} - X_CVX{1} )^2 + norm( xAll{2} - X_CVX{2} )^2 )/...
    sqrt( norm(X_CVX{1})^2 + norm(X_CVX{2})^2 ); % should be about .006

if er > 0.4 || mnConstraint1 < -.01 || mnConstraint2 < -.01
    error('Failed the test');
else
    disp('This test successfully passed');
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


% demo_geodet     December 16, 2012

% demonstrates the geocentric-to-geodetic functions

% Orbital Mechanics with Matlab

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

clear all;

global req flat

% conversion factor - degrees-to-radians

rtd = 180.0d0 / pi;

% Earth equatorial radius (kilometers)

req = 6378.1363;

% Earth flattening factor (non-dimensional)

flat = 1.0 / 298.257;

% Earth polar axis (kilometers)

rpolar = req * (1.0d0 - flat);

% eci position vector (kilometers)

reci(1) = -.586479273288D+04;
reci(2) = -.178173078828D+04;
reci(3) = -.215629990858D+04;

rmag = norm(reci);

dec = asin(reci(3) / rmag);

[alt1, lat1] = geodet1 (rmag, dec);

clc; home;

fprintf('geocentric declination    %14.8f  degrees \n\n', rtd * dec);

fprintf('geocentric radius         %14.8f  kilometers \n\n', rmag);

fprintf('\ngeodet1 function\n');
fprintf('================\n\n');

fprintf('geodetic latitude         %14.8f  degrees \n\n', rtd * lat1);

fprintf('geodetic altitude         %14.8f  kilometers \n\n', alt1);

[lat2, alt2] = geodet2(dec, rmag);

fprintf('\ngeodet2 function\n');
fprintf('================\n\n');

fprintf('geodetic latitude         %14.8f  degrees \n\n', rtd * lat2);

fprintf('geodetic altitude         %14.8f  kilometers \n\n', alt2);

[alt5, lat5] = geodet5 (req, rpolar, reci);

fprintf('\ngeodet5 function\n');
fprintf('================\n\n');

fprintf('geodetic latitude         %14.8f  degrees \n\n', rtd * lat5);

fprintf('geodetic altitude         %14.8f  kilometers \n\n', alt5);

[alt6, lat6] = geodet6 (req, rpolar, reci);

fprintf('\ngeodet6 function\n');
fprintf('================\n\n');

fprintf('geodetic latitude         %14.8f  degrees \n\n', rtd * lat6);

fprintf('geodetic altitude         %14.8f  kilometers \n\n', alt6);


function RFparam = RF_train(RFparam)
%% RFparam.D: input dimension
%% RFparam.M: desired output dimension
%% RFparam.gamma: bandwidth of the Gaussian kernel

%% actually, there is no training here. This function is just randomly setting the
%% code parameters.

RFparam.R = randn(RFparam.D,RFparam.M)*sqrt(RFparam.gamma);
RFparam.B = rand(1,RFparam.M) * 2 * pi;
%RFparam.T = zeros(1,RFparam.M);
RFparam.T = (rand(1,RFparam.M) * 2 - 1) * sqrt(2);


function y = softmax(x)
    M = bsxfun(@minus, x, max(x, [], 1));
    numerator = exp(M);
    denominator = sum(numerator) + 0.00001;
    y = bsxfun(@rdivide, numerator, denominator) + 0.00001;
end


function [M] = spm_mci_priors (M)
% Quantities for computing log prior in subspace
% FORMAT [M] = spm_mci_priors (M)
%
% M.V               projection matrix
% M.ipC             Inverse prior cov in reduced space
% M.log_prior_t2    second term of log prior
% M.Np              dimension of reduced space
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_mci_priors.m 6548 2015-09-11 12:39:47Z will $

if isstruct(M.pC)
    pC=full(diag(spm_vec(M.pC)));
else
    pC = M.pC;
end
V  = spm_svd(pC,exp(-32));
Np = size(V,2);
pC = V'*pC*V;
ipC = inv(pC);
log_prior_t2 = spm_logdet(ipC)/2-0.5*Np*log(2*pi);

M.ipC=ipC;
M.V=V;
M.log_prior_t2=log_prior_t2;
M.Np=Np;


function matrix = genMatCurl3D(pde,mesh,fem)
%% Generate global matrices and load vector of FEM for 3D ellipic eq
%     -div(A grad u)  = f,    x\in \Omega
% INPUTS:
% pde --- given data function from equation, e.g.
%         pde.A --- diffusion coefficient
%         pde.f --- right hand side function
%         pde.gD --- Dirichlet boundary value function
%         pde.one --- constant function 1.
% mesh --- mesh structure.
% fem --- global degree of freedom of FEM
%
% OUTPUTS:
% matrix.S --- stiffness matrix (w/o boundary condition)
% matrix.A --- final FEM matrix (after boundary condition)
% matrix.rhsF --- load vector (w/o boundary condition)
% matrix.f --- final RHS matrix (after boundary condition)

% Last Modified: 08/07/2020 by Xu Zhang

%% 1. Stiffness Matrix
S = globMatrixNed3D(pde.A,1,mesh,fem,fem);
M = globMatrixNed3D(pde.B,0,mesh,fem,fem);

%% 2. Generate the Right Hand Side Vector
rhsF1 = globNedRHS3D(pde.f1, mesh, fem, 0, 1);
rhsF2 = globNedRHS3D(pde.f2, mesh, fem, 0, 2);
rhsF3 = globNedRHS3D(pde.f3, mesh, fem, 0, 3);
rhsF = rhsF1 + rhsF2 + rhsF3;

%% 3. Dirichlet Boundary Conditions
Atotal = S + M;
tu1 = sum(feval(pde.exactu1,fem.gex,fem.gey,fem.gez).*fem.gew,2);
tu2 = sum(feval(pde.exactu2,fem.gex,fem.gey,fem.gez).*fem.gew,2);
tu3 = sum(feval(pde.exactu3,fem.gex,fem.gey,fem.gez).*fem.gew,2);
tgt = mesh.p(mesh.e(:,2),:) - mesh.p(mesh.e(:,1),:);
tgt = tgt./sum(tgt.^2,2).^(1/2);
tu = tu1.*tgt(:,1) + tu2.*tgt(:,2) + tu3.*tgt(:,3);
ub = tu;
ub(fem.mapper) = 0;
rhsB = Atotal*ub;
A = Atotal(fem.mapper,fem.mapper);
f = rhsF(fem.mapper) - rhsB(fem.mapper);

%% Outputs
matrix = struct('A', A, 'f', f, 'S', S, 'rhsF',rhsF,'tu',tu);


function x=v_pcma2lin(p,m,s)
%V_PCMU2LIN Convert A-law PCM to linear X=(P,M,S)
%	lin = v_pcma2lin(pcma,m,s) where pcma contains a vector or matrix
%	of A-law values in the range 0 to 255.
%	No checking is performed to see that numbers are in this range.
%
%	Input values are exclusive ored with m (default=85)
%
%	Output values are divided by the scale factor s:
%
%		   s		Output Range
%
%		   1		+-4032	(integer values)
%		2017.396342	+-1.998616 (default)
%		4032		+-1
%		4096		+-0.984375 (+-1 nominal full scale)
%
%	The default value of s is 2017.396342 which equals
%	sqrt((1120^2 + 2624^2)/2). This factor follows ITU standard G.711 and
%	the sine wave with PCM-A values [225 244 244 225 97 116 116 97]
%	has a mean square value of unity corresponding to 0 dBm0.

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: v_pcma2lin.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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<3
  t=4.95688418E-4;
  if nargin<2 m=85; end
else
  t=1/s;
end

if m q=bitxor(p,m); else q=p; end;
k=rem(q,16);
g=floor(q/128);
e=(q-k-128*g)/16;
f=(abs(e-1)-e+1)/2;
x=(2*g-1).*(pow2(k+16.5,e)+f.*(k-15.5))*t;


function D = DLSI_updateD(D, E, F, A, lambda, opts)
% function D = DLSI_updateD(D, E, F, A, lambda, opts)
% problem: `D = argmin_D -2trace(ED') + trace(FD'*D) + lambda *||A*D||F^2,`
% subject to: `||d_i||_2^2 <= 1`
% where F is a positive semidefinite matrix
% ========= aproach: ADMM ==============================
% rewrite: `[D, Z] = argmin -2trace(ED') + trace(FD'*D) + lambda ||A*Z||_F^2,`
%     subject to `D = Z; ||d_i||_2^2 <= 1`
% aproach 1: ADMM.
% 1. D = -2trace(ED') + trace(FD'*D) + rho/2 ||D - Z + U||_F^2,
%     s.t. ||d_i||_2^2 <= 1
% 2. Z = argmin lambda*||A*Z|| + rho/2||D - Z + U||_F^2
% 3. U = U + D - Z
% solve 1: D = argmin -2trace(ED') + trace(FD'*D) + rho/2 ||D - W||_F^2
%                       with W = Z - U;
%            = argmin -2trace((E - rho/2*W)*D') +
%               trace((F + rho/2 * eye())*D'D)
% solve 2: derivetaive: 0 = 2A'AZ + rho (Z - V) with V = D + U
% `Z = B*rhoV` with `B = (2*lambda*A'*A + rho I)^{-1}`
% `U = U + D - Z`
% -----------------------------------------------
% Author: Tiep Vu, thv102@psu.edu, 5/11/2016
%         (http://www.personal.psu.edu/thv102/)
% -----------------------------------------------

    if nargin == 0
        clc;
        d = 300;
        N = 10;
        k = 5;
        k2 = 495;
        load('tmp.mat');
        lambda = 0.01;

        opts.show = 0;
        opts.max_iter = 300;
        opts.verbose = 1;

    end
    if nargin == 6
        opts.lambda = lambda;
    elseif nargin == 5
        opts = lambda;
    end
    %%
    function cost = calcost(D)
        cost =  -2*trace(E*D') + trace(F*D'*D) + lambda*normF2(A*D);
    end
    %%
    iter = 0;
    rho = 1.0;
    Z_old = D;
    U = zeros(size(D));
    I_k = eye(size(D,2));
    % B = inv(2*lambda*A'*A + rho*I_k2); However, this might be very expensive if size(A, 2) is big, which is common
    % Instead, we can use the Sherman–Morrison formula at
    % https://en.wikipedia.org/wiki/Sherman%E2%80%93Morrison_formula#Generalization_(Woodbury_Matrix_Identity)
    X = 2*lambda/rho*A';
    Y = A;
    B1 = X*inv(eye(size(Y, 1)) + Y*X);
    tol = 1e-5;
    optsD.max_iter = 100;
    optsD.tol = 1e-8;
    while iter < opts.max_iter
        iter = iter + 1;
        %% ========= update D ==============================
        W = Z_old - U;
        E2 = E + rho/2 * W;
        F2 = F + rho/2*I_k;
        D = ODL_updateD(D, E2, F2, optsD);
        %% ========= update Z ==============================
        V = D + U;
        % Z_new = rho*B*V; slow
        Z_new = rho*(V - B1*(Y*V)); % fast
        e1 = normF2(D - Z_new);
        e2 = rho*normF2(Z_new - Z_old);
        if (e1 < optsD.tol && e2 < optsD.tol)
            break;
        end
        if opts.verbose
            cost = calcost(D);
            fprintf('iter = %3d | costD = %5.4f | normF2(D - Z) = %5.4f | rho(Z_new - Z_old) = %5.4f\n', iter, cost, e1, e2);
        end
        %% ========= update U ==============================
        U = U + D - Z_new;
        Z_old = Z_new;
    end
%     disp(t1)
%     disp(t2)
    if nargin == 0
        D = [];
    end
end


function B = rowNormalize(A)
% DESCRIPTION: normalize each row of the matrix
A_n_r = sqrt(sum(A.^2,2,'omitnan'));
B = A./repmat(A_n_r,1,size(A,2));
end


function [err,time,solver,eqn] = femPoisson3(mesh,pde,option,varargin)
%% FEMPOISSON3 solve Poisson equation by various finite element methods
%
%   FEMPOISSON3 computes approximations to the Poisson equation on a
%   sequence of meshes obtained by uniform refinement of a input mesh.
%
% See also Poisson, crack, Lshape
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

%% Check input arguments
if isfield(mesh,'node') && isfield(mesh,'elem')
    node = mesh.node;
    elem = double(mesh.elem);
else
    [node,elem] = cubemesh([0,1,0,1,0,1],0.25); % default mesh is a cube
end
if isfield(mesh,'bdFlag')
    bdFlag = mesh.bdFlag;
else
    bdFlag = setboundary3(node,elem,'Dirichlet');
end
if ~exist('option','var'), option = []; end
if ~exist('pde','var')
    pde = sincosdata3;                          % default data
end

%% Parameters
option = femoption(option);
maxIt = option.maxIt;
maxN = option.maxN;
L0 = option.L0;
elemType = option.elemType;
refType = option.refType;

%% Generate an initial mesh
for k = 1:L0
    if strcmp(refType,'red')
        [node,elem,bdFlag] = uniformrefine3(node,elem,bdFlag);
    elseif strcmp(refType,'bisect')
        [node,elem,bdFlag] = uniformbisect3(node,elem,bdFlag);
    end
end

%% Initialize err
errL2 = zeros(maxIt,1);   errH1 = zeros(maxIt,1);
erruIuh = zeros(maxIt,1); errMax = zeros(maxIt,1);
errTime = zeros(maxIt,1); solverTime = zeros(maxIt,1);
assembleTime = zeros(maxIt,1); meshTime = zeros(maxIt,1);
itStep = zeros(maxIt,1);  stopErr = zeros(maxIt,1); flag = zeros(maxIt,1);
N = zeros(maxIt,1); h = zeros(maxIt,1);

%% Finite Element Method
for k = 1:maxIt
%     bdFlag = sparse(double(bdFlag));
    % solve the equation
    switch elemType
        case 'P1'     % piecewise linear function P1 element
            [soln,eqn,info] = Poisson3(node,elem,bdFlag,pde,option);
        case 'CR'     % piecewise linear function CR element
            [soln,eqn,info] = Poisson3CR(node,elem,bdFlag,pde,option);
        case 'P2'     % piecewise quadratic function
            [soln,eqn,info] = Poisson3P2(node,elem,bdFlag,pde,option);
        case 'WG'     % weak Galerkin element
            [soln,eqn,info] = Poisson3WG(node,elem,bdFlag,pde,option);
    end
    uh = soln.u;
    % compute error
    t = cputime;
    if isfield(pde,'Du')
        if isfield(soln,'Du') && ~isempty(soln.Du) % Du is in the output
            errH1(k) = getH1error3(node,elem,pde.Du,soln.Du);
        else
            errH1(k) = getH1error3(node,elem,pde.Du,uh);
        end
    end
    if isfield(pde,'exactu')
        errL2(k) = getL2error3(node,elem,pde.exactu,uh);
        % interpolation
        switch elemType
            case 'P1'
                uI = Lagrangeinterpolate(pde.exactu,node,elem);
            case 'CR'
                uI = Lagrangeinterpolate(pde.exactu,node,elem,'CR',eqn.face);
            case 'P2'
                uI = Lagrangeinterpolate(pde.exactu,node,elem,'P2',eqn.edge);
            case 'WG'
                uI = Lagrangeinterpolate(pde.exactu,node,elem,'WG',eqn.face);
        end
        erruIuh(k) = sqrt((uh-uI)'*eqn.A*(uh-uI));
        errMax(k) = max(abs(uh-uI));
    end
    errTime(k) = cputime - t;
    % record time
    solverTime(k) = info.solverTime;
    assembleTime(k) = info.assembleTime;
    if option.printlevel>1
        fprintf('Time to compute the error %4.2g s \n H1 err %4.2g    L2err %4.2g \n',...
                 errTime(k), errH1(k), errL2(k));
    end
    % record solver information
    itStep(k) = info.itStep;
    stopErr(k) = info.stopErr;
    flag(k) = info.flag;
    % plot
    N(k) = length(soln.u);
    h(k) = 1./(size(node,1)^(1/3)-1);
    if  strcmp(elemType,'WG') % modify size for WG
        if ~isfield(option,'reducesystem') || (option.reducesystem == 1)
            N(k) = N(k) - size(elem,1); % reduced system
        end
    end
    if option.plotflag && N(k) < 2e4 % show mesh and solution for small size
        switch elemType
        case 'P1'     % piecewise linear function P1 element
            figure(1);  showresult3(node,elem,uh);
        case 'CR'     % piecewise linear function CR element
            continue;
        case 'P2'     % piecewise quadratic function
            figure(1);  showresult3(node,elem,uh(1:size(node,1)));
        case 'WG'     % weak Galerkin element
            continue;
        end
    end
    if N(k) > maxN
        break;
    end
    % refine mesh
    t = cputime;
    if strcmp(refType,'red')
        [node,elem,bdFlag] = uniformrefine3(node,elem,bdFlag);
    elseif strcmp(refType,'bisect')
        [node,elem,bdFlag] = uniformbisect3(node,elem,bdFlag);
    end
    meshTime(k) = cputime - t;
end

%% Plot convergence rates
if option.rateflag
    figure;
    set(gcf,'Units','normal');
    set(gcf,'Position',[0.25,0.25,0.55,0.4]);
    subplot(1,2,1)
    showrateh2(h(1:k),errH1(1:k),1,'-*','||Du-Du_h||',...
               h(1:k),errL2(1:k),1,'k-+','||u-u_h||');
    title(['Convergence Rate of 3D ', elemType, '-element']);
    subplot(1,2,2)
    showrateh2(h(1:k),erruIuh(1:k),1,'m-+','||Du_I-Du_h||',...
               h(1:k),errMax(1:k),1,'r-*','||u_I-u_h||_{\infty}');
    title(['Convergence Rate of 3D ', elemType, '-element']);
end

%% Output
err = struct('h',h(1:k),'N',N,'H1',errH1(1:k),'L2',errL2(1:k),...
             'uIuhH1',erruIuh(1:k),'uIuhMax',errMax(1:k));
time = struct('N',N,'err',errTime(1:k),'solver',solverTime(1:k), ...
              'assemble',assembleTime(1:k),'mesh',meshTime(1:k));
solver = struct('N',N(1:k),'itStep',itStep(1:k),'time',solverTime(1:k),...
                'stopErr',stopErr(1:k),'flag',flag(1:k));

%% Display error and time
if option.dispflag
    disp('Table: Error')
    colname = {'#Dof','h','||u-u_h||','||Du-Du_h||','||DuI-Du_h||','||uI-u_h||_{max}'};
    disptable(colname,err.N,[],err.h,'%0.3e',err.L2,'%0.5e',err.H1,'%0.5e',...
              err.uIuhH1,'%0.5e',err.uIuhMax,'%0.5e');

    disp('Table: CPU time')
    colname = {'#Dof','Assemble','Solve','Error','Mesh'};
    disptable(colname,time.N,[],time.assemble,'%0.2e',time.solver,'%0.2e',...
                      time.err,'%0.2e',time.mesh,'%0.2e');
end


function visible_probability = hidden_state_to_visible_probabilities(rbm_w, hidden_state)
% <rbm_w> is a matrix of size <number of hidden units> by <number of visible units>
% <hidden_state> is a binary matrix of size <number of hidden units> by <number of configurations that we're handling in parallel>.
% The returned value is a matrix of size <number of visible units> by <number of configurations that we're handling in parallel>.
% This takes in the (binary) states of the hidden units, and returns the activation probabilities of the visible units, conditional on those states.
    m_ = rbm_w' * hidden_state;
    visible_probability = 1 ./ (1 + e .^ (-m_));
end


% Created for the blood glucose step response
% by John D. Hedengren
% 02 Sept 2003

global u A

% Steady State Initial Conditions for the States
% Basal values of glucose and insulin conc.
G_ss = 4.5; % mmol/L
X_ss = 15; % mU/L
I_ss = 15; % mU/L

x_ss = [G_ss;I_ss;X_ss];

% Steady State Initial Condition for the Control
u_ss = 16.667; % mU/min

% Steady State for the Disturbance
d_ss = 0; % mmol/L-min

% Final Time (min)
tf = 400;

[t,x] = ode15s('blood_glucose',[0 tf],x_ss);

% Separate out the state values
G = x(:,1);
X = x(:,2);
I = x(:,3);

% Plot the results
figure(1);
plot(t,G);
legend('Glucose');
xlabel('Time (min)');
ylabel('mmol/L');

figure(2);
plot(t,I,t,X);
legend('Plasma Insulin','Remote Compartment Insulin');
xlabel('Time (min)');
ylabel('mU/L');


%kvlabel 'Perform Labeling on Multiband or Cluster Image (K1)'
% This MatLab function was automatically generated by a converter (KhorosToMatLab) from the Khoros vlabel.pane file
%
% Parameters:
% Integer: w 'Border Width ', default: 0: 'Specify the border width in pixels'
% MultiChoice: m 'Type of Distance Metric (Choose One): ', default: 1: 'Distance Metric: 1 = Euclidean distance, 2 = city block'
%    Choices are:
%   1: 'Euclidean '
%   2: 'City Block'
% MultiChoice: c 'Type of Connectivity (Choose One): ', default: 1: 'Connectivity: 1 = 4-connectivity, 2 = 8-connectivity'
%    Choices are:
%   1: '4 Connectivity'
%   2: '8 Connectivity'
% Double: s 'Minimum Region Size in % ', default: 1: 'Minimum number of pixel to keep in label region'
% OutputFile: o 'Output Labeled Image', required: 'output image after the labeling process'
% OutputFile: asc 'Output ASCII File', optional: 'Output ASCII file for vlabel result display'
% InputFile: i1 'Initial Image ', optional: 'input image'
% InputFile: i2 'Cluster Center Image', optional: 'input image for cluster centers'
% InputFile: i3 'Cluster Number Image', optional: 'input image for cluster numbers'
% Integer: n 'Approximate Number of Regions Expected ', default: 10: 'Number of region to get from labeling process'
% Double: f 'Split and Merge Factor', default: 0.07: 'Split & Merge Factor: higher values result in finer regions'
%
% Example: [o, asc] = kvlabel({i1, i2, i3}, {'w',0;'m',1;'c',1;'s',1;'o','';'asc','';'i1','';'i2','';'i3','';'n',10;'f',0.07})
%
% Khoros helpfile follows below:
%
%  PROGRAM
% vlabel - Perform Labeling on Multiband or Cluster Image  (K1)
%
%  DESCRIPTION
% .I vlabel
% performs a labeling on a multiband image or a cluster image by
% attempting to merge connected pixels.
%
% The principal of the algorithm is as follows: A pixel receives the same
% label as its neighbor if the likelihood distance between the two pixels
% is acceptable.  The label process is propagated for a given region number
% until it is no longer possible to find a candidate.
%
% \fBThree different types of labeling choices exist":
%
%  1
% .I First choice:
% uses a single or multi band image, where the data storage can be
% any of the following types: VFF_TYP_1_BYTE, VFF_TYP_2_BYTE,
% VFF_TYP_4_BYTE, or VFF_TYP_FLOAT.
% The input image corresponds to the -i1 argument.
% The distance is computed using all the bands of the image.
%
%  2.
% .I Second choice:
% uses a cluster number and cluster center
% image obtained from an algorithm like vkmeans or vquant.
% For this case, the cluster center represents the value of a
% class of pixels that have been grouped together.
% Therefore, the distance between two neighbors will be the
% distance between their clusters.
% This case will require less computation time because the algorithm
% will only compute the inter-class distance, instead of computing
% the distance of two neighbors for the entire image.
% An additional advantage with this choice, is that the output from
% algorithms such as vkmeans or vquant may be utilized, which may lead
% to better results.
% The cluster center image corresponds to -i2 argument
% The cluster number image corresponds to -i3 argument in the
% command line.
%
%  3.
% .I The final possibility
% is to use a single or multiband input image
% (argument -i1) associated with a cluster number image (argument -i3).
% The advantage of this choice is that the results of a clustering
% algorithm are used to keep the neighbor pixels that have the same cluster
% number in the same class, and to rely on the distance in the single or
% multi band image to group two neighbors that do not belong to the same
% cluster.
%
% In summary, the three possible choices are:
%
%
% -i1 image.xv
%
%
% -i2 cluster_center.xv -i3 cluster_number.xv
%
%
% -i1 image.xv -i3 cluster_number.xv
%
% .I The algorithm also requires
% the following parameters:
% \fBMetric distance:"
% There are 2 different metric distances that can be used.
%
%
% -d 1 uses  Euclidean distance: sqrt[(x-s)^2 + (y-t)^2].
%
%
% -d 2 uses  City Block distance: |x-s| + |y-t|.
%
% \fBConnectivity:"
% There are two possible neighborhoods:
%
%
% -c 1  uses the 4 connectivity to link pixels together.
%
%
% -c 2  uses the 8 connectivity.
%
% \fBMinimum size of a region:"
%
%
% -s  (float_value)  determines the number of pixels required for a region
% to be retained.
% The minimum number of pixels is equal to:
% Total number of pixels in image * float_value / 100.0 (-s corresponds to
% a percentage of the total number of pixels in the image).
%
% \fBBorder Size:"
% Each pixel in the image is updated except those outside of the border.
% The size of the border is specified by the -w argument.
%
% \fBMerging Process:"
% When the labeling process is computed, the user can expect that the small
% rejected region will be merged together in a bigger acceptable region or
% will be included inside another connected region.
% This choice is selected by setting the logical argument -merge to TRUE.
% If -merge is set to FALSE, the small regions will be ignored and labeled as
% an UNDEFINED REGION (label number 0), the same as the border.
%
% "The AUTOMATIC or MANUAL OPTION:\fP
% This option allows the user either to fix a threshold, or to give
% an approximate number of regions.
% If the AUTOMATIC option is used, the algorithm will iterate on the threshold
% until the number of regions labeled by the process is comparable to the number
% of expected regions.
% In fact, if the expected number is not reached after 30 iterations,
% the threshold that gives the closest number of regions is used for
% the final labeling.
%
% Although this option is easy to use, the function:
%
%
% number of regions = F(Threshold)  is not a monotonically increasing function,
% and the convergence toward a solution may not exist.
%
%
% -n  int_value   (AUTOMATIC OPTION) determines the approximate final number of
% regions expected.
% -f  float_value (MANUAL OPTION) determines the threshold used by the labeling
% process.
% THESE TWO OPTIONS ARE MUTUALLY EXCLUSIVE
% The default threshold value is 0.07. This value generally gives good results
% on noiseless images with large uniform regions.
% Decreasing this value will increase the number of regions found during the
% labeling process, but these regions will get smaller and could be rejected
% by the minimum size threshold.
% Increasing this value will decrease the number of regions found during the
% labeling process. At the same time, the number of small regions will decrease
% which means that this area of the curve, \fBnumber of regions = F(Threshold)",
% is more stable than the other one.
% Once the user becomes accustomed to this routine, good results are generally
% obtained.  One way to become familiar with the routine, is to use the
% automatic option and analyze the output ASCII file (Statistics on the
% iteration process). This file contains the number of regions labeled for
% each iteration, allowing the user to see how the number of regions changes
% as the THRESHOLD is changed.
%
% \fBOutput Files"
%
%  1
% \fBOutput image:"
% The resulting image, which corresponds to the -o1 argument, contains the
% labeled image in which every pixel has a region number as its value. This
% image is of data storage type VFF_TYP_4_BYTE.
% The region label numbers are 1 to N.  The region number 0 is reserved
% as an UNDEFINED label or for the border.
%
%  2
% \fBOutput Statistic ASCII File:"
% This file contains all the information relative to the labeling process.
%
%
%
%
%  EXAMPLES
%
% vlabel -i1 image.xv -d 1 c 1 -merge 0 -f 0.07 -s 0.7 -w 2 -o image1.xv -asc stats
% This command will label image.xv using the Euclidean distance, the
% 4 connectivity, a split and merge factor equal to 0.07, a minimum
% size for the regions equal to 0.7 percent of the total number of pixels
% in image.xv, and a border size of 2. The merge option is not used which
% means that the small regions will not be labeled.
% The labeled image will be stored in image1.xv and the statistics will be
% written in the ASCII file, stats.
%
%  "SEE ALSO"
% lvkmeans(3), lvquant(3), vkmeans(1), vquant(1).
%
%  RESTRICTIONS
% vlabel works only with cluster number and cluster center images conforming
% to the convention established by the set of clustering algorithms.
%
%  REFERENCES
%
%  COPYRIGHT
% Copyright (C) 1993 - 1997, Khoral Research, Inc. ("KRI")  All rights reserved.
%

function varargout = kvlabel(varargin)
if nargin ==0
  Inputs={};arglist={'',''};
elseif nargin ==1
  Inputs=varargin{1};arglist={'',''};
elseif nargin ==2
  Inputs=varargin{1}; arglist=varargin{2};
else error('Usage: [out1,..] = kvlabel(Inputs,arglist).');
end
if size(arglist,2)~=2
  error('arglist must be of form {''ParameterTag1'',value1;''ParameterTag2'',value2}')
 end
narglist={'w', 0;'m', 1;'c', 1;'s', 1;'o', '__output';'asc', '__output';'i1', '__input';'i2', '__input';'i3', '__input';'n', 10;'f', 0.07};
maxval={16,0,0,100,0,1,1,1,1,2500,1};
minval={0,0,0,0,0,1,1,1,1,2,0};
istoggle=[1,0,0,1,0,1,1,1,1,1,1];
was_set=istoggle * 0;
paramtype={'Integer','MultiChoice','MultiChoice','Double','OutputFile','OutputFile','InputFile','InputFile','InputFile','Integer','Double'};
% identify the input arrays and assign them to the arguments as stated by the user
if ~iscell(Inputs)
Inputs = {Inputs};
end
NumReqOutputs=1; nextinput=1; nextoutput=1;
  for ii=1:size(arglist,1)
  wasmatched=0;
  for jj=1:size(narglist,1)
   if strcmp(arglist{ii,1},narglist{jj,1})  % a given argument was matched to the possible arguments
     wasmatched = 1;
     was_set(jj) = 1;
     if strcmp(narglist{jj,2}, '__input')
      if (nextinput > length(Inputs))
        error(['Input ' narglist{jj,1} ' has no corresponding input!']);
      end
      narglist{jj,2} = 'OK_in';
      nextinput = nextinput + 1;
     elseif strcmp(narglist{jj,2}, '__output')
      if (nextoutput > nargout)
        error(['Output nr. ' narglist{jj,1} ' is not present in the assignment list of outputs !']);
      end
      if (isempty(arglist{ii,2}))
        narglist{jj,2} = 'OK_out';
      else
        narglist{jj,2} = arglist{ii,2};
      end

      nextoutput = nextoutput + 1;
      if (minval{jj} == 0)
         NumReqOutputs = NumReqOutputs - 1;
      end
     elseif isstr(arglist{ii,2})
      narglist{jj,2} = arglist{ii,2};
     else
        if strcmp(paramtype{jj}, 'Integer') & (round(arglist{ii,2}) ~= arglist{ii,2})
            error(['Argument ' arglist{ii,1} ' is of integer type but non-integer number ' arglist{ii,2} ' was supplied']);
        end
        if (minval{jj} ~= 0 | maxval{jj} ~= 0)
          if (minval{jj} == 1 & maxval{jj} == 1 & arglist{ii,2} < 0)
            error(['Argument ' arglist{ii,1} ' must be bigger or equal to zero!']);
          elseif (minval{jj} == -1 & maxval{jj} == -1 & arglist{ii,2} > 0)
            error(['Argument ' arglist{ii,1} ' must be smaller or equal to zero!']);
          elseif (minval{jj} == 2 & maxval{jj} == 2 & arglist{ii,2} <= 0)
            error(['Argument ' arglist{ii,1} ' must be bigger than zero!']);
          elseif (minval{jj} == -2 & maxval{jj} == -2 & arglist{ii,2} >= 0)
            error(['Argument ' arglist{ii,1} ' must be smaller than zero!']);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} < minval{jj})
            error(['Argument ' arglist{ii,1} ' must be bigger than ' num2str(minval{jj})]);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} > maxval{jj})
            error(['Argument ' arglist{ii,1} ' must be smaller than ' num2str(maxval{jj})]);
          end
        end
     end
     if ~strcmp(narglist{jj,2},'OK_out') &  ~strcmp(narglist{jj,2},'OK_in')
       narglist{jj,2} = arglist{ii,2};
     end
   end
   end
   if (wasmatched == 0 & ~strcmp(arglist{ii,1},''))
        error(['Argument ' arglist{ii,1} ' is not a valid argument for this function']);
   end
end
% match the remaining inputs/outputs to the unused arguments and test for missing required inputs
 for jj=1:size(narglist,1)
     if  strcmp(paramtype{jj}, 'Toggle')
        if (narglist{jj,2} ==0)
          narglist{jj,1} = '';
        end;
        narglist{jj,2} = '';
     end;
     if  ~strcmp(narglist{jj,2},'__input') && ~strcmp(narglist{jj,2},'__output') && istoggle(jj) && ~ was_set(jj)
          narglist{jj,1} = '';
          narglist{jj,2} = '';
     end;
     if strcmp(narglist{jj,2}, '__input')
      if (minval{jj} == 0)  % meaning this input is required
        if (nextinput > size(Inputs))
           error(['Required input ' narglist{jj,1} ' has no corresponding input in the list!']);
        else
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        end
      else  % this is an optional input
        if (nextinput <= length(Inputs))
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end;
     else
     if strcmp(narglist{jj,2}, '__output')
      if (minval{jj} == 0) % this is a required output
        if (nextoutput > nargout & nargout > 1)
           error(['Required output ' narglist{jj,1} ' is not stated in the assignment list!']);
        else
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
          NumReqOutputs = NumReqOutputs-1;
        end
      else % this is an optional output
        if (nargout - nextoutput >= NumReqOutputs)
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end
     end
  end
end
if nargout
   varargout = cell(1,nargout);
else
  varargout = cell(1,1);
end
global KhorosRoot
if exist('KhorosRoot') && ~isempty(KhorosRoot)
w=['"' KhorosRoot];
else
if ispc
  w='"C:\Program Files\dip\khorosBin\';
else
[s,w] = system('which cantata');
w=['"' w(1:end-8)];
end
end
[varargout{:}]=callKhoros([w 'vlabel"  '],Inputs,narglist);


function [b, r, crit] = stepwise(X, y, criterion)

    if nargin<3, criterion = 'BIC'; end

    % qr factorization will speed up stepwise regression significantly
    [Q,R] = qr(X,0); % note that the zero is very important for performance
    invR = pinv(R);

    n = size(y,1);
    LL = nan(size(X,2),1);
    for i = 1:length(LL)
        % get residual for each fit
        b = invR(1:i,1:i) * Q(:,1:i)' * y;
        r = y - X(:,1:i)*b;

        % calculate log-likelihood
        LL(i) = -n/2*log( 2*pi*mean(r.^2) ) - n/2;

    end

    % Calculate information criterion
    crit = infocrit( LL , n , (1:length(LL))' , criterion );

    % optimal model order
    lst=find(~isnan(crit));
    [~, N] = min( crit(lst) );
    N=lst(N);

    % finally, our output
    b = invR(1:N,1:N) * Q(:,1:N)'*y;
    r = y - X(:,1:N)*b;

end


% A simple tutorial file to interface with RF
% Options copied from http://cran.r-project.org/web/packages/randomForest/randomForest.pdf

%run plethora of tests
clc
close all

%compile everything
if strcmpi(computer,'PCWIN') |strcmpi(computer,'PCWIN64')
   compile_windows
else
   compile_linux
end

total_train_time=0;
total_test_time=0;

%load the twonorm dataset
load data/twonorm

%modify so that training data is NxD and labels are Nx1, where N=#of
%examples, D=# of features

X = inputs';
Y = outputs;

[N D] =size(X);
%randomly split into 250 examples for training and 50 for testing
randvector = randperm(N);

X_trn = X(randvector(1:250),:);
Y_trn = Y(randvector(1:250));
X_tst = X(randvector(251:end),:);
Y_tst = Y(randvector(251:end));

% example 1:  simply use with the defaults
    model = classRF_train(X_trn,Y_trn);
    Y_hat = classRF_predict(X_tst,model);
    fprintf('\nexample 1: error rate %f\n',   length(find(Y_hat~=Y_tst))/length(Y_tst));

% example 2:  set to 100 trees
    model = classRF_train(X_trn,Y_trn, 100);
    Y_hat = classRF_predict(X_tst,model);
    fprintf('\nexample 2: error rate %f\n',   length(find(Y_hat~=Y_tst))/length(Y_tst));

% example 3:  set to 100 trees, mtry = 2
    model = classRF_train(X_trn,Y_trn, 100,2);
    Y_hat = classRF_predict(X_tst,model);
    fprintf('\nexample 3: error rate %f\n',   length(find(Y_hat~=Y_tst))/length(Y_tst));

% example 4:  set to defaults trees and mtry by specifying values as 0
    model = classRF_train(X_trn,Y_trn, 0, 0);
    Y_hat = classRF_predict(X_tst,model);
    fprintf('\nexample 4: error rate %f\n',   length(find(Y_hat~=Y_tst))/length(Y_tst));

% example 5: set sampling without replacement (default is with replacement)
    extra_options.replace = 0 ;
    model = classRF_train(X_trn,Y_trn, 100, 4, extra_options);
    Y_hat = classRF_predict(X_tst,model);
    fprintf('\nexample 5: error rate %f\n',   length(find(Y_hat~=Y_tst))/length(Y_tst));

% example 6: Using classwt (priors of classes)
    clear extra_options;
    extra_options.classwt = [1 1]; %for the [-1 +1] classses in twonorm
    % if you sort the labels in training and arrange in ascending order then
    % for twonorm you have -1 and +1 classes, with here assigning 1 to
    % both classes
    % As you have specified the classwt above, what happens that the priors are considered
    % also is considered the freq of the labels in the data. If you are
    % confused look into src/rfutils.cpp in normClassWt() function

    model = classRF_train(X_trn,Y_trn, 100, 4, extra_options);
    Y_hat = classRF_predict(X_tst,model);
    fprintf('\nexample 6: error rate %f\n',   length(find(Y_hat~=Y_tst))/length(Y_tst));

% example 7: modify to make class(es) more IMPORTANT than the others
    %  extra_options.cutoff (Classification only) = A vector of length equal to
    %                       number of classes. The 'winning' class for an observation is the one with the maximum ratio of proportion
    %                       of votes to cutoff. Default is 1/k where k is the number of classes (i.e., majority
    %                       vote wins).    clear extra_options;
    extra_options.cutoff = [1/4 3/4]; %for the [-1 +1] classses in twonorm
    % if you sort the labels in training and arrange in ascending order then
    % for twonorm you have -1 and +1 classes, with here assigning 1/4 and
    % 3/4 respectively
    % thus the second class needs a lot less votes to win compared to the first class

    model = classRF_train(X_trn,Y_trn, 100, 4, extra_options);
    Y_hat = classRF_predict(X_tst,model);
    fprintf('\nexample 7: error rate %f\n',   length(find(Y_hat~=Y_tst))/length(Y_tst));
    fprintf('   y_trn is almost 50/50 but y_hat now has %f/%f split\n',length(find(Y_hat~=-1))/length(Y_tst),length(find(Y_hat~=1))/length(Y_tst));

%  extra_options.strata = (not yet stable in code) variable that is used for stratified
%                       sampling. I don't yet know how this works.

% example 8: sampsize example
    %  extra_options.sampsize =  Size(s) of sample to draw. For classification,
    %                   if sampsize is a vector of the length the number of strata, then sampling is stratified by strata,
    %                   and the elements of sampsize indicate the numbers to be drawn from the strata.
    clear extra_options
    extra_options.sampsize = size(X_trn,1)*2/3;

    model = classRF_train(X_trn,Y_trn, 100, 4, extra_options);
    Y_hat = classRF_predict(X_tst,model);
    fprintf('\nexample 8: error rate %f\n',   length(find(Y_hat~=Y_tst))/length(Y_tst));

% example 9: nodesize
    %  extra_options.nodesize = Minimum size of terminal nodes. Setting this number larger causes smaller trees
    %                   to be grown (and thus take less time). Note that the default values are different
    %                   for classification (1) and regression (5).
    clear extra_options
    extra_options.nodesize = 2;

    model = classRF_train(X_trn,Y_trn, 100, 4, extra_options);
    Y_hat = classRF_predict(X_tst,model);
    fprintf('\nexample 9: error rate %f\n',   length(find(Y_hat~=Y_tst))/length(Y_tst));

% example 10: calculating importance
    clear extra_options
    extra_options.importance = 1; %(0 = (Default) Don't, 1=calculate)

    model = classRF_train(X_trn,Y_trn, 100, 4, extra_options);
    Y_hat = classRF_predict(X_tst,model);
    fprintf('\nexample 10: error rate %f\n',   length(find(Y_hat~=Y_tst))/length(Y_tst));

    %model will have 3 variables for importance importanceSD and localImp
    %importance = a matrix with nclass + 2 (for classification) or two (for regression) columns.
    %           For classification, the first nclass columns are the class-specific measures
    %           computed as mean decrease in accuracy. The nclass + 1st column is the
    %           mean decrease in accuracy over all classes. The last column is the mean decrease
    %           in Gini index. For Regression, the first column is the mean decrease in
    %           accuracy and the second the mean decrease in MSE. If importance=FALSE,
    %           the last measure is still returned as a vector.
    figure('Name','Importance Plots')
    subplot(2,1,1);
    bar(model.importance(:,end-1));xlabel('feature');ylabel('magnitude');
    title('Mean decrease in Accuracy');

    subplot(2,1,2);
    bar(model.importance(:,end));xlabel('feature');ylabel('magnitude');
    title('Mean decrease in Gini index');

    %importanceSD = The ?standard errors? of the permutation-based importance measure. For classification,
    %           a D by nclass + 1 matrix corresponding to the first nclass + 1
    %           columns of the importance matrix. For regression, a length p vector.
    model.importanceSD

% example 11: calculating local importance
    %  extra_options.localImp = Should casewise importance measure be computed? (Setting this to TRUE will
    %                   override importance.)
    %localImp  = a D by N matrix containing the casewise importance measures, the [i,j] element
    %           of which is the importance of i-th variable on the j-th case. NULL if
    %          localImp=FALSE.
    clear extra_options
    extra_options.localImp = 1; %(0 = (Default) Don't, 1=calculate)

    model = classRF_train(X_trn,Y_trn, 100, 4, extra_options);
    Y_hat = classRF_predict(X_tst,model);
    fprintf('\nexample 11: error rate %f\n',   length(find(Y_hat~=Y_tst))/length(Y_tst));

    model.localImp

% example 12: calculating proximity
    %  extra_options.proximity = Should proximity measure among the rows be calculated?
    clear extra_options
    extra_options.proximity = 1; %(0 = (Default) Don't, 1=calculate)

    model = classRF_train(X_trn,Y_trn, 100, 4, extra_options);
    Y_hat = classRF_predict(X_tst,model);
    fprintf('\nexample 12: error rate %f\n',   length(find(Y_hat~=Y_tst))/length(Y_tst));

    model.proximity

% example 13: use only OOB for proximity
    %  extra_options.oob_prox = Should proximity be calculated only on 'out-of-bag' data?
    clear extra_options
    extra_options.proximity = 1; %(0 = (Default) Don't, 1=calculate)
    extra_options.oob_prox = 0; %(Default = 1 if proximity is enabled,  Don't 0)

    model = classRF_train(X_trn,Y_trn, 100, 4, extra_options);
    Y_hat = classRF_predict(X_tst,model);
    fprintf('\nexample 13: error rate %f\n',   length(find(Y_hat~=Y_tst))/length(Y_tst));

% example 14: to see what is going on behind the scenes
%  extra_options.do_trace = If set to TRUE, give a more verbose output as randomForest is run. If set to
%                   some integer, then running output is printed for every
%                   do_trace trees.
    clear extra_options
    extra_options.do_trace = 1; %(Default = 0)

    model = classRF_train(X_trn,Y_trn, 100, 4, extra_options);
    Y_hat = classRF_predict(X_tst,model);
    fprintf('\nexample 14: error rate %f\n',   length(find(Y_hat~=Y_tst))/length(Y_tst));

% example 14: to see what is going on behind the scenes
%  extra_options.keep_inbag Should an n by ntree matrix be returned that keeps track of which samples are
%                   'in-bag' in which trees (but not how many times, if sampling with replacement)
%
    clear extra_options
    extra_options.keep_inbag = 1; %(Default = 0)

    model = classRF_train(X_trn,Y_trn, 100, 4, extra_options);
    Y_hat = classRF_predict(X_tst,model);
    fprintf('\nexample 15: error rate %f\n',   length(find(Y_hat~=Y_tst))/length(Y_tst));

    model.inbag

% example 16: getting the OOB rate. model will have errtr whose first
% column is the OOB rate. and the second column is for the 1-st class and
% so on
    model = classRF_train(X_trn,Y_trn);
    Y_hat = classRF_predict(X_tst,model);
    fprintf('\nexample 16: error rate %f\n',   length(find(Y_hat~=Y_tst))/length(Y_tst));

    figure('Name','OOB error rate');
    plot(model.errtr(:,1)); title('OOB error rate');  xlabel('iteration (# trees)'); ylabel('OOB error rate');

% example 17: getting prediction per tree, votes etc for test set
    model = classRF_train(X_trn,Y_trn);

    test_options.predict_all = 1;
    [Y_hat, votes, prediction_pre_tree] = classRF_predict(X_tst,model,test_options);
    fprintf('\nexample 17: error rate %f\n',   length(find(Y_hat~=Y_tst))/length(Y_tst));


function results = vl_test_kmeans(varargin)
% VL_TEST_KMEANS

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

vl_test_init ;

function s = setup()
randn('state',0) ;
s.X = randn(128, 100) ;

function test_basic(s)
[centers, assignments, en] = vl_kmeans(s.X, 10, 'NumRepetitions', 10) ;
[centers_, assignments_, en_] = simpleKMeans(s.X, 10) ;
assert(en_ <= 1.1 * en, 'vl_kmeans did not optimize enough') ;

function test_algorithms(s)
distances = {'l1', 'l2'} ;
dataTypes = {'single','double'} ;

for dataType = dataTypes
  for distance = distances
    distance = char(distance) ;
    conversion = str2func(char(dataType)) ;
    X = conversion(s.X) ;
    vl_twister('state',0) ;
    [centers, assignments, en] = vl_kmeans(X, 10, ...
                                           'NumRepetitions', 1, ...
                                           'MaxNumIterations', 10, ...
                                           'Algorithm', 'Lloyd', ...
                                           'Distance', distance) ;
    vl_twister('state',0) ;
    [centers_, assignments_, en_] = vl_kmeans(X, 10, ...
                                              'NumRepetitions', 1, ...
                                              'MaxNumIterations', 10, ...
                                              'Algorithm', 'Elkan', ...
                                              'Distance', distance) ;
    vl_assert_almost_equal(centers, centers_, 1e-5) ;
    vl_assert_almost_equal(assignments, assignments_, 1e-5) ;
    vl_assert_almost_equal(en, en_, 1e-5) ;
  end
end

function test_patterns(s)
distances = {'l1', 'l2'} ;
dataTypes = {'single','double'} ;
for dataType = dataTypes
  for distance = distances
    distance = char(distance) ;
    conversion = str2func(char(dataType)) ;
    data = [1 1 0 0 ;
            1 0 1 0] ;
    data = conversion(data) ;
    [centers, assignments, en] = vl_kmeans(data, 4, ...
                                           'NumRepetitions', 100, ...
                                           'Distance', distance) ;
    assert(isempty(setdiff(data', centers', 'rows'))) ;
  end
end

function [centers, assignments, en] = simpleKMeans(X, numCenters)
[dimension, numData] = size(X) ;
centers = randn(dimension, numCenters) ;

for iter = 1:10
  [dists, assignments] = min(vl_alldist(centers, X)) ;
  en = sum(dists) ;
  centers = [zeros(dimension, numCenters) ; ones(1, numCenters)] ;
  centers = vl_binsum(centers, ...
                      [X ; ones(1,numData)], ...
                      repmat(assignments, dimension+1, 1), 2) ;
  centers = centers(1:end-1, :) ./ repmat(centers(end,:), dimension, 1) ;
end


function K = rbf(kern,dat1,dat2,ind1,ind2,kerParam),

% K = rbf(d1,d2,ind1,ind2,param), compute the kernel
%     matrix between d1 and d2
% for a rbf kernel exp(-||x-z||^2/(2*param^2))
%     where x is from d1 and z from d2

  K=get_x(dat2,ind2)*get_x(dat1,ind1)';
  kernTemp=kernel;
  Kdn = get_norm(kernTemp,dat1,ind1).^2;
  Kn = get_norm(kernTemp,dat2,ind2).^2;
  K = ones(length(Kn),1)*Kdn' + Kn*ones(1,length(Kdn)) - 2*K;
  K = exp(-K/(2*kerParam^2));


%compute central speed in the central-head direction
function [data,units]=compute_velcentralch(trx,n)

larvae=trx.exp2flies{n};
numlarvae=numel(larvae);
velcentralch=cell(1,numlarvae);
for i=1:numlarvae
    larva=larvae(i);
    % this is just slightly faster
    velcentralch{i} = trx(larva).dxcentral_mm.*cos(trx(larva).centralheadang(1:end-1)) + ...
      trx(larva).dycentral_mm.*sin(trx(larva).centralheadang(1:end-1));
    %velcentralch1{1,i}=trx(larva).velmagcentral.*(cos(trx(larva).velangcentral).*cos(trx(larva).centralheadang(1,1:end-1))+sin(trx(larva).velangcentral).*sin(trx(larva).centralheadang(1,1:end-1)));
end

units=parseunits('mm/s');
data=velcentralch;


function [ npart, a, rank ] = partn_successor ( n, nmax, npart, a, rank )

%*****************************************************************************80
%
%% PARTN_SUCCESSOR computes partitions whose largest part is NMAX.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer N, the integer to be partitioned.
%    N must be positive.
%
%    Input, integer NMAX, the maximum size of any part of the
%    partition.  1 <= NMAX <= N.
%
%    Input/output, integer NPART, the number of parts of the
%    partition.  1 <= NPART <= N.
%
%    Input/output, integer A(N), contains the partition.
%    A(1) through A(NPART) contain the nonzero integers which
%    sum to N.
%
%    Input/output, integer RANK, the rank.
%    If RANK = -1 on input, then the routine understands that this is
%    the first call, and that the user wishes the routine to supply
%    the first element in the ordering, which has RANK = 0.
%    In general, the input value of RANK is increased by 1 for output,
%    unless the very last element of the ordering was input, in which
%    case the output value of RANK is 0.
%

%
%  Return the first element.
%
  if ( rank == -1 )
    a(1) = nmax;
    npart = n + 1 - nmax;
    a(2:npart) = 1;
    rank = 0;
    return
  end
%
%  Check.
%
  ierror = partn_sf_check ( n, nmax, npart, a );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PARTN_SUCCESSOR - Fatal error!\n' );
    fprintf ( 1, '  The input array is illegal.\n' );
    fprintf ( 1, '  IERROR = %d\n', ierror );
    error ( 'PARTN_SUCCESSOR - Fatal error!' );
  end
%
%  If there are at least two parts, and the next to last is not NMAX,
%  then rob the last part and pay the next to the last part.
%  Then, if the next to last part is too big, swap it leftwards.
%
  if ( 1 < npart )

    if ( a(npart-1) < nmax )

      a(npart) = a(npart) - 1;
      a(npart-1) = a(npart-1) + 1;
      index = npart - 1;

      while ( 1 )

        if ( index <= 1 )
          break
        end

        if ( a(index) <= a(index-1) )
          break
        end

        temp       = a(index-1);
        a(index-1) = a(index);
        a(index)   = temp;

        index = index - 1;

      end
%
%  Sum the tail.
%
      temp = sum ( a(index+1:npart) );
%
%  Spread the sum as 1's.
%
      npart = index + temp;
      a(index+1:npart) = 1;
      rank = rank + 1;
      return

    end
%
%  Otherwise, we've reached the last item.
%  Return the first one.
%
  else

    npart = n + 1 - nmax;
    a(1) = nmax;
    a(2:npart) = 1;
    rank = 0;
    return

  end

  return
end


function s = sharpness(im, metric)
%SHARPNESS   Calculate image sharpness metric.
%
% DESCRIPTION:
%       sharpness returns a scalar metric related the sharpness of the 2D
%       or 3D image matrix defined by im. By default, the metric is based
%       on the Brenner gradient which returns the sum of the centered
%       finite-difference at each matrix element in each Cartesian
%       direction. Metrics calculated using the Sobel operator or the
%       normalised variance can also be returned by setting the input
%       paramater metric.
%
%       For further details, see B. E. Treeby, T. K. Varslot, E. Z. Zhang,
%       J. G. Laufer, and P. C. Beard, "Automatic sound speed selection in
%       photoacoustic image reconstruction using an autofocus approach," J.
%       Biomed. Opt., vol. 16, no. 9, p. 090501, 2011.
%
% USAGE:
%       s = sharpness(im)
%       s = sharpness(im, metric)
%
% INPUTS:
%       im          - 2D or 3D image data to evaluate
%
% OPTIONAL INPUTS:
%       metric      - sharpness metric. Supported values are:
%                       'Brenner'           (default)
%                       'Tenenbaum'
%                       'NormVariance'
%
% OUTPUTS:
%       s           - computed sharpness metric
%
% ABOUT:
%       author      - Bradley Treeby
%       date        - 15th January 2012
%       last update - 15th January 2012
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

% This file is part of k-Wave. k-Wave 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.
%
% k-Wave 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 Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

% check for metric input
if nargin == 1
   metric = 'Brenner';
end

switch metric

    case 'Brenner'

        % compute sharpness metric based on the Brenner gradient
        switch numDim(im)
            case 2

                % compute metric
                bren_x = (im(1:end-2, :) - im(3:end, :)).^2;
                bren_y = (im(:, 1:end-2) - im(:, 3:end)).^2;
                s = sum(bren_x(:)) + sum(bren_y(:));

            case 3

                % compute metric
                bren_x = (im(1:end-2, :, :) - im(3:end, :, :)).^2;
                bren_y = (im(:, 1:end-2, :) - im(:, 3:end, :)).^2;
                bren_z = (im(:, :, 1:end-2) - im(:, :, 3:end)).^2;
                s = sum(bren_x(:)) + sum(bren_y(:)) + sum(bren_z(:));

        end

    case 'Tenenbaum'

        % compute sharpness metric based on the Tenenbaum gradient
        switch numDim(im)
            case 2

                % define the 2D sobel gradient operator
                sobel = [-1 0 1; -2 0 2; -1 0 1];

                % compute metric
                s = conv2(sobel, im).^2 + conv2(sobel.', im).^2;
                s = sum(s(:));

            case 3

                % define the 3D sobel gradient operator
                sobel3D(:, :, 1) = [1 2 1; 2 4 2; 1 2 1];
                sobel3D(:, :, 2) = zeros(3);
                sobel3D(:, :, 3) = -sobel3D(:, :, 1);

                % compute metric
                s = convn(im, sobel3D).^2 + convn(im, permute(sobel3D, [3 1 2])).^2 +  convn(im, permute(sobel3D, [2 3 1])).^2;
                s = sum(s(:));

        end

    case 'NormVariance'

        % compute sharpness metric based on the normalised variance
        mu = mean(im(:));
        s = sum((im(:) - mu).^2)./mu;

end


%% This file will generate the guess of the iSINDYc left hand side.
% Last Updated: 2019/05/08
% Coded By: K
%
% Note that this file is modified. When the iter = 6, we will add dx6*x6^4
% to the left hand side guess.
function [P_Data,P_sym]=GuessLib(X,dX,iter,u,Highest_Poly_Order,Highest_Trig_Order,Highest_U_Order)
%% First get the size of the X matrix, determin the data length and the number of variables we have.
[Data_Length,Variable_Number]=size(X);
[~,Variable_Number_dX]=size(dX);
[~,Variable_Number_u]=size(u);
% Also create the symbolic variable
Symbol=sym('z',[Variable_Number,1]);
Symbol_dX=sym('dz',[Variable_Number,1]);
Symbol_u=sym('u',[Variable_Number_u,1]);

%% Now according the Highest Polynomial Order entered, we will calculate the data matrix.
Data=[];
Index=1;

%% First calculate the polynomial term

%Order Zero:
Data(:,Index)=ones(Data_Length,1);
Sym_Struct{1,Index}=sym(1);

%Order One:
if Highest_Poly_Order>=1
    for i=1:Variable_Number
        Index=Index+1;
        Data(:,Index)=X(:,i);
        Sym_Struct{1,Index}=Symbol(i,1);
    end
end

%Order Two:
if Highest_Poly_Order>=2
    for i=1:Variable_Number
        for j=i:Variable_Number
            Index=Index+1;
            Data(:,Index)=X(:,i).*X(:,j);
            Sym_Struct{1,Index}=Symbol(i,1)*Symbol(j,1);
        end
    end
end

%Order Three:
if Highest_Poly_Order>=3
    for i=1:Variable_Number
        for j=1:Variable_Number
            for k=1:Variable_Number
                Index=Index+1;
                Data(:,Index)=X(:,i).*X(:,j).*X(:,k);
                Sym_Struct{1,Index}=Symbol(i,1)*Symbol(j,1)*Symbol(k,1);
            end
        end
    end
end

%Order Four:
if Highest_Poly_Order>=4
    for i=1:Variable_Number
        for j=1:Variable_Number
            for k=1:Variable_Number
                for p=1:Variable_Number
                    Index=Index+1;
                    Data(:,Index)=X(:,i).*X(:,j).*X(:,k).*X(:,p);
                    Sym_Struct{1,Index}=Symbol(i,1)*Symbol(j,1)*Symbol(k,1)*Symbol(p,1);
                end
            end
        end
    end
end

%% Then add the Trigonometric Function in the output data:

%Order One:
if Highest_Trig_Order>=1
    for i=1:Variable_Number
        Index=Index+1;
        Data(:,Index)=sin(X(:,i));
        Sym_Struct{1,Index}=sin(Symbol(i,1));
    end
    for i=1:Variable_Number
        Index=Index+1;
        Data(:,Index)=cos(X(:,i));
        Sym_Struct{1,Index}=cos(Symbol(i,1));
    end
end

%Order Two:
if Highest_Trig_Order>=2
    %
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(X(:,i)-X(:,j)).^2;
            Sym_Struct{1,Index}=sin(Symbol(i,1)-Symbol(j,1))^2;
        end
    end
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(X(:,i)-X(:,j)).^2;
            Sym_Struct{1,Index}=cos(Symbol(i,1)-Symbol(j,1))^2;
        end
    end
end

%Order Three:
if Highest_Trig_Order>=3
    %
    for i=1:Variable_Number
        for j=1:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(X(:,i)+X(:,j));
            Sym_Struct{1,Index}=sin(Symbol(i,1)+Symbol(j,1));
        end
    end
    for i=1:Variable_Number
        for j=1:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(X(:,i)+X(:,j));
            Sym_Struct{1,Index}=cos(Symbol(i,1)+Symbol(j,1));
        end
    end
    %
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(X(:,i)-X(:,j));
            Sym_Struct{1,Index}=sin(Symbol(i,1)-Symbol(j,1));
        end
    end
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(X(:,i)-X(:,j));
            Sym_Struct{1,Index}=cos(Symbol(i,1)-Symbol(j,1));
        end
    end
end

%Order Four:
if Highest_Trig_Order>=4
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(X(:,i)-X(:,j)).*X(:,i).^2;
            Sym_Struct{1,Index}=sin(Symbol(i,1)-Symbol(j,1))*Symbol(i,1)^2;
        end
    end
    %
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(X(:,i)-X(:,j)).*X(:,i).^2;
            Sym_Struct{1,Index}=cos(Symbol(i,1)-Symbol(j,1))*Symbol(i,1)^2;
        end
    end
    %
    for i=1:Variable_Number
        for j=1:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(X(:,i)-2*X(:,j));
            Sym_Struct{1,Index}=sin(Symbol(i,1)-2*Symbol(j,1));
        end
    end
    %
    for i=1:Variable_Number
        for j=1:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(X(:,i)-2*X(:,j));
            Sym_Struct{1,Index}=cos(Symbol(i,1)-2*Symbol(j,1));
        end
    end
    %
    for i=1:Variable_Number
        for j=i:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(2*X(:,i)-2*X(:,j)).*X(:,i).^2;
            Sym_Struct{1,Index}=sin(2*Symbol(i,1)-2*Symbol(j,1))*Symbol(i,1)^2;
        end
    end
    %
    for i=1:Variable_Number
        for j=i:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(2*X(:,i)-2*X(:,j)).*X(:,i).^2;
            Sym_Struct{1,Index}=cos(2*Symbol(i,1)-2*Symbol(j,1))*Symbol(i,1)^2;
        end
    end
end

% Add the guess x6^4 manually for the selected states.
if iter==1 || iter==2 || iter==6
    Index=Index+1;
    Data(:,Index)=X(:,6).^4;
    Sym_Struct{1,Index}=Symbol(6,1)^4;
end

pin=Index;
j=0;
%% Frome here, we add the right hand side to our data

%If the Highest_U_Order is zero, we assume that our system does not have
%control input, vice versa.
if Highest_U_Order==0
    for k=1:pin
        j=j+1;
        P_Data(:,j)=dX(:,1).*Data(:,k);
        P_sym{1,j}=Symbol_dX(iter,1)*(Sym_Struct{1,k});
    end
else
    for i=1:Variable_Number_u
        for k=1:pin
            j=j+1;
            P_Data(:,j)=u(:,1).*Data(:,k);
            P_sym{1,j}=Symbol_u(1,1)*(Sym_Struct{1,k});
        end
    end
end


function alpha=solveAlpha(I,consts_map,consts_vals,varargin)

  [h,w,c]=size(I);
  img_size=w*h;

  A=getLaplacian1(I,consts_map,varargin{:});

  D=spdiags(consts_map(:),0,img_size,img_size);
  lambda=100;
  x=(A+lambda*D)\(lambda*consts_map(:).*consts_vals(:));

  alpha=max(min(reshape(x,h,w),1),0);


%
% Usage:  [W [W2]]=mexStochasticProx(y,X,W0,param);
%
% Name: mexStochasticProx
%
% Description: mexStochasticProx implements a proximal MM stochastic algorithm
% for composite optimization in a large scale setting.
%        X is a design matrix of size p x n
%        y is a vector of size n
% WARNING, X is transposed compared to the functions mexFista*, and y is a vector
%        param.lambda is a vector that contains nlambda different values of the
%        regularization parameter (it can be a scalar, in that case nlambda=1)
%        W0: is a dense matrix of size p x nlambda   It is in fact ineffective
%        in the current release
%        W: is the output, dense matrix of size p x nlambda
%
%         - if param.loss='square' and param.regul corresponds to a regularization
%           function (currently 'l1' or 'l2'), the following problem is solved
%           w = argmin (1/n)sum_{i=1}^n 0.5(y_i- x_i^T w)^2 + lambda psi(w)
%         - if param.loss='logistic' and param.regul corresponds to a regularization
%           function (currently 'l1' or 'l2'), the following problem is solved
%           w = argmin (1/n)sum_{i=1}^n log(1+ exp(-y_ix_i^T w)) + lambda psi(w)
%           Note that here, the y_i's should be -1 or +1
%
%         The current release does not handle intercepts
%
% Inputs: y: double dense vector of size n
%         X: dense or sparse matrix of size p x n
%         W0: dense matrix of size p x nlambda
%         param: struct
%           param.loss (choice of loss)
%           param.regul (choice of regularization function)
%           param.lambda : vector of size nlambda
%           param.iters : number of iterations (n corresponds to one pass over the data)
%           param.minibatches: size of the mini-batches: recommended value is 1
%           param.normalized : (optional, can be set to true if the x_i's have
%              unit l2-norm, false by default)
%           param.weighting_mode : (optional, 1 by default),
%                0:  w_t = (t_0+1)/(t+t_0)
%                1:  w_t = ((t_0+1)/(t+t_0))^(0.75)
%                2:  w_t = ((t_0+1)/(t+t_0))^(5)
%           param.averaging_mode: (optional, false by default)
%                0: no averaging
%                1: first averaging mode for W2
%                2: second averaging mode for W2
%                WARNING: averaging can be very slow for sparse solutions
%           param.determineEta (optional, automatically choose the parameters of the
%             learning weights w_t, true by default)
%           param.t0 (optional, set up t0 for weights w_t = ((1+t0)/(t+t0))^(alpha)
%           param.numThreads (optional, number of threads)
%           param.verbose (optional)
%           param.seed (optional, choice of the random seed)
%
% Output:  W:  double dense p x nlambda matrix (contains the solution without averaging)
%          W2:  double dense p x nlambda matrix (contains the solution with averaging)
%
% Author: Julien Mairal, 2013


function [cm] = mm2cm(mm)
% Convert length from millimeters to centimeters.
% Chad A. Greene 2012
cm = mm/10;


function p = createMultiframeTestphan()

% load the k-space of a predifined multiecho testphantom from file
basePath = fileparts(mfilename('fullpath'));
dat = load(fullfile(basePath, 'testPhan64.mat'));
p = dat.p;

% create a movie with different (simulated) shifts of the phantom
NF = 30; % number of frames
dim = size(p,1);
P = zeros([size(p),NF]);

x = linspace(0,1,dim);
[X,Y] = meshgrid(x,x');
shiftMax = dim * 2*pi;
shiftInc = shiftMax / NF;

shift = 0;
for i = 1 : NF
    shiftPhX = exp(1i * -shift * X);
    shiftPhY = exp(1i * shift * Y);
    shift = shift + shiftInc;
    currP = ftimes(p,shiftPhX);
    currP = ftimes(currP,shiftPhY);
    P(:,:,:,i) = currP;
end

% do a Fourier reconstruction of the data
P = asDataClass.mrIfft(P);

% simulate 4 different coil profiles
coils = cat(5,X,Y,1-X, 1-Y);
P = ftimes(P,coils);

% simulate phase perturbations
phase = exp(1i *2 * pi * (1.5 - coils));
P = ftimes(P,phase);

% create pseudo k-space data from the fft of the images
p = asDataClass.mrFft(P);
end


function y = normalizeAngle(u)
%#codegen
% normalizes angle to -pi to pi
y = u;

y(u>pi) = mod(u(u>pi) + pi, 2*pi) - pi;
y(u<-pi) = -(mod(-(u(u<-pi) - pi), 2*pi) - pi);


clear all; close all;
I=zeros(200, 200);
I(50:150, 50:150)=1;
theta=0:10:180;
[R, xp]=radon(I, theta);
figure;
subplot(121);
imshow(I);
subplot(122);
imagesc(theta, xp, R);
colormap(hot);
colorbar;


function [textures] = getGLSZMtextures_STS(GLSZM)
% -------------------------------------------------------------------------
% function [textures] = getGLSZMtextures_STS(GLSZM)
% -------------------------------------------------------------------------
% DESCRIPTION:
% This function computes texture features from an input Gray-Level Size
% Zone Matrix (GLSZM). (STS study)
% -------------------------------------------------------------------------
% REFERENCES:
% [1] Galloway, M. M. (1975). Texture analysis using gray level run lengths.
%     Computer Graphics and Image Processing, 4(2), 172–179.
% [2] Chu, A., Sehgal, C. M., & Greenleaf, J. F. (1990). Use of gray value
%     distribution of run lengths for texture analysis. Pattern Recognition
%     Letters, 11(6), 415-419.
% [3] Dasarathy, B. V., & Holder, E. B. (1991). Image characterizations
%     based on joint gray level-run length distributions. Pattern
%     Recognition Letters, 12(8), 497-502.
% [4] Thibault, G., Fertil, B., Navarro, C., Pereira, S., Cau, P., Levy,
%     N., Mari, J.-L. (2009). Texture Indexes and Gray Level Size Zone
%     Matrix. Application to Cell Nuclei Classification. In Pattern
%     Recognition and Information Processing (PRIP) (pp. 140–145).
% -------------------------------------------------------------------------
% INPUTS:
% - GLSZM: Gray-Level Size Zone Matrix.
%
% ** 'GLSZM' should be the output from 'getGLSZM.m' **
% -------------------------------------------------------------------------
% OUTPUTS:
% - textures: Struture specifying the values of different GLSZM texture
%             features as defined below.
% -------------------------------------------------------------------------
% AUTHOR(S): Martin Vallieres <mart.vallieres@gmail.com>
% -------------------------------------------------------------------------
% HISTORY:
% - Creation: January 2013
% - Revision: May 2015
% -------------------------------------------------------------------------
% STATEMENT:
% This file is part of <https://github.com/mvallieres/radiomics/>,
% a package providing MATLAB programming tools for radiomics analysis.
% --> Copyright (C) 2015  Martin Vallieres
%
%    This package is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    This package is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this package.  If not, see <http://www.gnu.org/licenses/>.
% -------------------------------------------------------------------------

% USEFUL MATRICES, VECTORS AND QUANTITIES
sz = size(GLSZM); % Size of GLSZM
nRuns = sum(GLSZM(:));
cVect = 1:sz(2); rVect = 1:sz(1);% Row and column vectors
[cMat,rMat] = meshgrid(cVect,rVect); % Column and row indicators for each entry of the GLSZM
pg = sum(GLSZM,2)'; % Gray-Level Run-Number Vector
pr = sum(GLSZM); % Run-Length Run-Number Vector

% COMPUTATION OF TEXTURE FEATURES
% 1. Small Zone Emphasis (SZE), Ref.[1,4]
textures.SZE = (pr*(cVect.^(-2))')/nRuns;

% 2. Large Zone Emphasis (LZE), Ref.[1,4]
textures.LZE = (pr*(cVect.^2)')/nRuns;

% 3. Gray-Level Nonuniformity (GLN), adapted from Ref.[1,4]
textures.GLN = sum(pg.^2)/nRuns;

% 4. Zone-Size Nonuniformity (ZSN), adapted from Ref.[1,4]
textures.ZSN = sum(pr.^2)/nRuns;

% 5. Zone Percentage (ZP), adapted from Ref.[1,4]
textures.ZP = nRuns/(pr*cVect');

% 6. Low Gray-Level Zone Emphasis (LGZE), Ref.[2,4]
textures.LGZE = (pg*(rVect.^(-2))')/nRuns;

% 7. High Gray-Level Zone Emphasis (HGZE), Ref.[2,4]
textures.HGZE = (pg*(rVect.^2)')/nRuns;

% 8. Small Zone Low Gray-Level Emphasis (SZLGE), Ref.[3,4]
textures.SZLGE = sum(sum(GLSZM.*(rMat.^(-2)).*(cMat.^(-2))))/nRuns;

% 9. Small Zone High Gray-Level Emphasis (SZHGE), Ref.[3,4]
textures.SZHGE = sum(sum(GLSZM.*(rMat.^2).*(cMat.^(-2))))/nRuns;

% 10. Large Zone Low Gray-Level Emphasis (LZLGE), Ref.[3,4]
textures.LZLGE = sum(sum(GLSZM.*(rMat.^(-2)).*(cMat.^2)))/nRuns;

% 11. Large Zone High Gray-Level Emphasis (LZHGE), Ref.[3,4]
textures.LZHGE = sum(sum(GLSZM.*(rMat.^2).*(cMat.^2)))/nRuns;

% New features according to Ref.[4]
GLSZM = GLSZM./nRuns; % In the future, this operation will be applied at the beginning of the function
pg=sum(GLSZM,2)'; pr=sum(GLSZM);
ug = (pg*rVect')/(sz(1)*sz(2));
ur = (pr*cVect')/(sz(1)*sz(2));

% 12. Gray-Level Variance (GLV), adapted from Ref.[4]
GLV = 0;
for g = 1:sz(1)
    for r = 1:sz(2)
        GLV = GLV + (GLSZM(g,r)*g-ug)^2;
    end
end
textures.GLV = GLV/(sz(1)*sz(2));

% 13. Zone-Size Variance (ZSV), adapted from Ref.[4]
ZSV = 0;
for g = 1:sz(1)
    for r = 1:sz(2)
        ZSV = ZSV + (GLSZM(g,r)*r-ur)^2;
    end
end
textures.ZSV = ZSV/(sz(1)*sz(2));

end


function reg_filter = get_reg_filter(sz, target_sz, params, reg_window_edge)

% Compute the spatial regularization function and derive the corresponding
% filter operation used for the optimization

if nargin < 3 || isempty(reg_window_edge)
    reg_window_edge = params.reg_window_edge;
end

if params.use_reg_window
    % create weight window
    reg_window_power = params.reg_window_power;

    % normalization factor
    reg_scale = 0.5 * target_sz;

    % construct grid
    wrg = -(sz(1)-1)/2:(sz(1)-1)/2;
    wcg = -(sz(2)-1)/2:(sz(2)-1)/2;
    [wrs, wcs] = ndgrid(wrg, wcg);

    % construct the regukarization window
    reg_window = (reg_window_edge - params.reg_window_min) * (abs(wrs/reg_scale(1)).^reg_window_power + abs(wcs/reg_scale(2)).^reg_window_power) + params.reg_window_min;

    % compute the DFT and enforce sparsity
    reg_window_dft = fft2(reg_window) / prod(sz);
    reg_window_dft(abs(reg_window_dft) < params.reg_sparsity_threshold * max(abs(reg_window_dft(:)))) = 0;

    % do the inverse transform, correct window minimum
    reg_window_sparse = real(ifft2(reg_window_dft));
    reg_window_dft(1,1) = reg_window_dft(1,1) - prod(sz) * min(reg_window_sparse(:)) + params.reg_window_min;
    reg_window_dft = fftshift(reg_window_dft);

    % find the regularization filter by removing the zeros
    reg_filter = cast(real(reg_window_dft(~all(reg_window_dft==0,2), ~all(reg_window_dft==0,1))), 'like', params.data_type);
else
    % else use a scaled identity matrix
    reg_filter = cast(params.reg_window_min, 'like', params.data_type);
end


function B = bernoulli(N)
%BERNOULLI   Bernoulli polynomials as chebfuns.
%   B = BERNOULLI(N) returns a quasimatrix of the first N+1 Bernoulli
%   polynomials on [0,1].
%
%   Example (Bernolli numbers):
%      B = cheb.bernoulli(4);
%      format rat, B(0,:)

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

x = chebfun('x', [0,1]);

% Initalize the constant 1 as the first Bernoulli polynomial.
B = 0*x + 1;
% Compute the requested polynomials.
for j = 1:N
    B(:,j+1) = j*cumsum(B(:,j));
    B(:,j+1) = B(:,j+1) - sum(B(:,j+1));
end

end


function M = stiefelfactory(n, p, k)
% Returns a manifold structure to optimize over orthonormal matrices.
%
% function M = stiefelfactory(n, p)
% function M = stiefelfactory(n, p, k)
%
% The Stiefel manifold is the set of orthonormal nxp matrices. If k
% is larger than 1, this is the Cartesian product of the Stiefel manifold
% taken k times. The metric is such that the manifold is a Riemannian
% submanifold of R^nxp equipped with the usual trace inner product, that
% is, it is the usual metric.
%
% Points are represented as matrices X of size n x p x k (or n x p if k=1,
% which is the default) such that each n x p matrix is orthonormal,
% i.e., X'*X = eye(p) if k = 1, or X(:, :, i)' * X(:, :, i) = eye(p) for
% i = 1 : k if k > 1. Tangent vectors are represented as matrices the same
% size as points.
%
% By default, k = 1.
%
% See also: grassmannfactory rotationsfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:
%  July  5, 2013 (NB) : Added ehess2rhess.
%  Jan. 27, 2014 (BM) : Bug in ehess2rhess corrected.
%  June 24, 2014 (NB) : Added true exponential map and changed the randvec
%                       function so that it now returns a globally
%                       normalized vector, not a vector where each
%                       component is normalized (this only matters if k>1).

    if ~exist('k', 'var') || isempty(k)
        k = 1;
    end

    if k == 1
        M.name = @() sprintf('Stiefel manifold St(%d, %d)', n, p);
    elseif k > 1
        M.name = @() sprintf('Product Stiefel manifold St(%d, %d)^%d', n, p, k);
    else
        error('k must be an integer no less than 1.');
    end

    M.dim = @() k*(n*p - .5*p*(p+1));

    M.inner = @(x, d1, d2) d1(:).'*d2(:);

    M.norm = @(x, d) norm(d(:));

    M.dist = @(x, y) error('stiefel.dist not implemented yet.');

    M.typicaldist = @() sqrt(p*k);

    M.proj = @projection;
    function Up = projection(X, U)

        XtU = multiprod(multitransp(X), U);
        symXtU = multisym(XtU);
        Up = U - multiprod(X, symXtU);

% The code above is equivalent to, but much faster than, the code below.
%
%     Up = zeros(size(U));
%     function A = sym(A), A = .5*(A+A'); end
%     for i = 1 : k
%         Xi = X(:, :, i);
%         Ui = U(:, :, i);
%         Up(:, :, i) = Ui - Xi*sym(Xi'*Ui);
%     end

    end

    M.tangent = M.proj;

    % For Riemannian submanifolds, converting a Euclidean gradient into a
    % Riemannian gradient amounts to an orthogonal projection.
	M.egrad2rgrad = M.proj;

    M.ehess2rhess = @ehess2rhess;
    function rhess = ehess2rhess(X, egrad, ehess, H)
        XtG = multiprod(multitransp(X), egrad);
        symXtG = multisym(XtG);
        HsymXtG = multiprod(H, symXtG);
        rhess = projection(X, ehess - HsymXtG);
    end

    M.retr = @retraction;
    function Y = retraction(X, U, t)
        if nargin < 3
            t = 1.0;
        end
        Y = X + t*U;
        for i = 1 : k
            [Q, R] = qr(Y(:, :, i), 0);
            % The instruction with R assures we are not flipping signs
            % of some columns, which should never happen in modern Matlab
            % versions but may be an issue with older versions.
            Y(:, :, i) = Q * diag(sign(sign(diag(R))+.5));
        end
    end

    M.exp = @exponential;
    function Y = exponential(X, U, t)
        if nargin == 2
            t = 1;
        end
        tU = t*U;
        Y = zeros(size(X));
        for i = 1 : k
            % From a formula by Ross Lippert, Example 5.4.2 in AMS08.
            Xi = X(:, :, i);
            Ui = tU(:, :, i);
            Y(:, :, i) = [Xi Ui] * ...
                         expm([Xi'*Ui , -Ui'*Ui ; eye(p) , Xi'*Ui]) * ...
                         [ expm(-Xi'*Ui) ; zeros(p) ];
        end

    end

    M.hash = @(X) ['z' hashmd5(X(:))];

    M.rand = @random;
    function X = random()
        X = zeros(n, p, k);
        for i = 1 : k
            [Q, unused] = qr(randn(n, p), 0); %#ok<NASGU>
            X(:, :, i) = Q;
        end
    end

    M.randvec = @randomvec;
    function U = randomvec(X)
        U = projection(X, randn(n, p, k));
        U = U / norm(U(:));
    end

    M.lincomb = @lincomb;

    M.zerovec = @(x) zeros(n, p, k);

    M.transp = @(x1, x2, d) projection(x2, d);

    M.vec = @(x, u_mat) u_mat(:);
    M.mat = @(x, u_vec) reshape(u_vec, [n, p, k]);
    M.vecmatareisometries = @() true;

end

% Linear combination of tangent vectors
function d = lincomb(x, a1, d1, a2, d2) %#ok<INUSL>

    if nargin == 3
        d = a1*d1;
    elseif nargin == 5
        d = a1*d1 + a2*d2;
    else
        error('Bad use of stiefel.lincomb.');
    end

end


function y = r8mat_mv ( m, n, a, x )

%*****************************************************************************80
%
%% R8MAT_MV multiplies a matrix times a vector.
%
%  Discussion:
%
%    In FORTRAN90, this operation can be more efficiently carried
%    out by the command
%
%      Y(1:M) = MATMUL ( A(1:M,1:N), X(1:N) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of the matrix.
%
%    Input, real A(M,N), the M by N matrix.
%
%    Input, real X(N), the vector to be multiplied by A.
%
%    Output, real Y(M), the product A*X.
%
  y(1:m) = a(1:m,1:n) * x(1:n);

  return
end


function [ n_data, mu, sigma, x, fx ] = cauchy_cdf_values ( n_data )

%*****************************************************************************80
%
%% CAUCHY_CDF_VALUES returns some values of the Cauchy CDF.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      Needs["Statistics`ContinuousDistributions`"]
%      dist = CauchyDistribution [ mu, sigma ]
%      CDF [ dist, x ]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real MU, the mean of the distribution.
%
%    Output, real SIGMA, the variance of the distribution.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 12;

  fx_vec = [ ...
     0.5000000000000000E+00, ...
     0.8524163823495667E+00, ...
     0.9220208696226307E+00, ...
     0.9474315432887466E+00, ...
     0.6475836176504333E+00, ...
     0.6024163823495667E+00, ...
     0.5779791303773693E+00, ...
     0.5628329581890012E+00, ...
     0.6475836176504333E+00, ...
     0.5000000000000000E+00, ...
     0.3524163823495667E+00, ...
     0.2500000000000000E+00 ];

  mu_vec = [ ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.4000000000000000E+01, ...
     0.5000000000000000E+01 ];

  sigma_vec = [ ...
     0.5000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.4000000000000000E+01, ...
     0.5000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01 ];

  x_vec = [ ...
     0.1000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.4000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.3000000000000000E+01 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    mu = 0.0;
    sigma = 0.0;
    x = 0.0;
    fx = 0.0;
  else
    mu = mu_vec(n_data);
    sigma = sigma_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function cardan_poly_coef_test ( )

%*****************************************************************************80
%
%% CARDAN_POLY_COEF_TEST tests CARDAN_POLY_COEF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2008
%
%  Author:
%
%    John Burkardt
%
  n_max = 10;

  s = 1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CARDAN_POLY_COEF_TEST\n' );
  fprintf ( 1, '  CARDAN_POLY_COEF returns the coefficients of a\n' );
  fprintf ( 1, '  Cardan polynomial.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  We use the parameter S = %f\n', s );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Table of polynomial coefficients:\n' );
  fprintf ( 1, '\n' );

  for n = 0 : n_max
    c = cardan_poly_coef ( n, s );
    fprintf ( 1, '  %2d  ', n );
    for i = 0 : n
      fprintf ( 1, '  %9f', c(i+1) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function g_hofstadter_test ( )

%*****************************************************************************80
%
%% G_HOFSTADTER_TEST tests G_HOFSTADTER.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'G_HOFSTADTER_TEST\n' );
  fprintf ( 1, '  G_HOFSTADTER evaluates Hofstadter''s recursive\n' );
  fprintf ( 1, '  G function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     N   G(N)' );
  fprintf ( 1, '\n' );

  for i = 0 : 30
    g = g_hofstadter ( i );
    fprintf ( 1, '  %4d  %4d\n', i, g );
  end

  return
end


function Score = CalWHV(PopObj,RefPoint,Weight)
% Calculate the exact weighted hypervolume value of the population

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    [N,M] = size(PopObj);
    PopObj(any(PopObj>repmat(RefPoint,N,1),2),:) = [];
    if isempty(PopObj)
        Score = 0;
    else
        pl = sortrows(PopObj);
        S  = {1,pl};
        for k = 1 : M-1
            S_ = {};
            for i = 1 : size(S,1)
                Stemp = Slice(cell2mat(S(i,2)),k,RefPoint);
                for j = 1 : size(Stemp,1)
                    temp(1) = {cell2mat(Stemp(j,1))*cell2mat(S(i,1))};
                    temp(2) = Stemp(j,2);
                    S_      = Add(temp,S_);
                end
            end
            S = S_;
        end
        Score = 0;
        for i = 1 : size(S,1)
            p     = Head(cell2mat(S(i,2)));
            Score = Score + cell2mat(S(i,1))*abs(p(M)-RefPoint(M))*Weight(i);
        end
    end
end

function S = Slice(pl,k,RefPoint)
    p  = Head(pl);
    pl = Tail(pl);
    ql = [];
    S  = {};
    while ~isempty(pl)
        ql  = Insert(p,k+1,ql);
        p_  = Head(pl);
        cell_(1,1) = {abs(p(k)-p_(k))};
        cell_(1,2) = {ql};
        S   = Add(cell_,S);
        p   = p_;
        pl  = Tail(pl);
    end
    ql = Insert(p,k+1,ql);
    cell_(1,1) = {abs(p(k)-RefPoint(k))};
    cell_(1,2) = {ql};
    S  = Add(cell_,S);
end

function ql = Insert(p,k,pl)
    flag1 = 0;
    flag2 = 0;
    ql    = [];
    hp    = Head(pl);
    while ~isempty(pl) && hp(k) < p(k)
        ql = [ql;hp];
        pl = Tail(pl);
        hp = Head(pl);
    end
    ql = [ql;p];
    m  = length(p);
    while ~isempty(pl)
        q = Head(pl);
        for i = k : m
            if p(i) < q(i)
                flag1 = 1;
            else
                if p(i) > q(i)
                    flag2 = 1;
                end
            end
        end
        if ~(flag1 == 1 && flag2 == 0)
            ql = [ql;Head(pl)];
        end
        pl = Tail(pl);
    end
end

function p = Head(pl)
    if isempty(pl)
        p = [];
    else
        p = pl(1,:);
    end
end

function ql = Tail(pl)
    if size(pl,1) < 2
        ql = [];
    else
        ql = pl(2:end,:);
    end
end

function S_ = Add(cell_,S)
    n = size(S,1);
    m = 0;
    for k = 1 : n
        if isequal(cell_(1,2),S(k,2))
            S(k,1) = {cell2mat(S(k,1))+cell2mat(cell_(1,1))};
            m = 1;
            break;
        end
    end
    if m == 0
        S(n+1,:) = cell_(1,:);
    end
    S_ = S;
end


function [q,Qv] = v2q(v)

% V2Q Rotaiton vector to quaternion conversion.
%   [Q,Qv] = V2Q(V) returns the quaternion Q correscponding to the rotation
%   encoded in rotation vector V, and the associated Jacobian Qv = dQ/dV.

if nargout == 1

    [a,u] = v2au(v);
    q = au2q(a,u);

else
    a = sqrt(dot(v,v));

    if isnumeric(a) && a < 1e-6

        % Use small signal approximation:
        q = [1-norm(v)^2/8
            v(:)/2];

        Qv = [-1/4*v(:)'
            0.5*eye(3)];

    else

        [a,u,Av,Uv] = v2au(v);
        [q,Qa,Qu] = au2q(a,u);
        Qv = Qa*Av + Qu*Uv;

    end
end

return

%%
syms r s t real
v = [r;s;t];
[q,Qv] = v2q(v)

simplify(Qv - jacobian(q,v))

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

%   # START GPL LICENSE

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

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

%   # END GPL LICENSE


function Y = sample_cond_multinomial(X, M)
% SAMPLE_MULTINOMIAL Sample Y(i) ~ M(X(i), :)
% function Y = sample_multinomial(X, M)
%
% X(i) = i'th sample
% M(i,j) = P(Y=j | X=i) = noisy channel model
%
% e.g., if X is a binary image,
% Y = sample_multinomial(softeye(2, 0.9), X)
% will create a noisy version of X, where bits are flipped with probability 0.1

if any(X(:)==0)
  error('data must only contain positive integers')
end

Y = zeros(size(X));
for i=min(X(:)):max(X(:))
  ndx = find(X==i);
  Y(ndx) = sample_discrete(M(i,:), length(ndx), 1);
end