function [x, infos] = recursive_nmu(V, rank, in_options)
% Recursive non-negative matrix underapproximation (Recursive-NMU).
%
% The problem of interest is defined as
%
%       min || V - WH ||_F^2,
%       where
%       {V, W, H} > 0 and WH <= V.
%
% Inputs:
%       matrix      V
%       rank        rank
%       options     options
%           Cnorm       Choice of the norm 1 or 2, default = 2.
% Output:
%       w           solution of w
%       infos       information
%
% References:
%       N. Gillis and F. Glineur,
%       "Using Underapproximations for Sparse Nonnegative Matrix Factorization,"
%       Pattern Recognition 43 (4), pp. 1676-1687,
%       2010.
%
%       N. Gillis and R.J. Plemmons,
%       "Dimensionality Reduction, Classification, and Spectral Mixture Analysis
%       using Nonnegative Underapproximationm,"
%       Optical Engineering 50, 027001,
%       2011.
%
%
% This file is part of NMFLibrary.
%
% This file has been ported from
%       recursiveNMU.m at https://gitlab.com/ngillis/nmfbook/-/tree/master/algorithms
%       by Nicolas Gillis (nicolas.gillis@umons.ac.be)
%
% Ported by T.Fukunaga and H.Kasai on June 24, 2022 for NMFLibrary
%
% Change log:
%
%

    % set dimensions and samples
    [m, n] = size(V);
    % set local options
    local_options = [];
    local_options.Cnorm = 2;
    local_options.inner_max_epoch = 200;

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

    % initialize factors
    init_options = options;
    [init_factors, ~] = generate_init_factors(V, rank, init_options);
    W = init_factors.W;
    H = init_factors.H;
    M = V;

    % initialize
    method_name = 'Recursive-NMU';
    epoch = 0;
    grad_calc_count = 0;

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

    % select disp_freq
    disp_freq = set_disp_frequency(options);

    % initialize for this algorithm
    % (here)

    % store initial info
    clear infos;
    [infos, f_val, optgap] = store_nmf_info(V, W, H, [], options, [], epoch, grad_calc_count, 0);

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

    % set start time
    start_time = tic();
    prev_time = start_time;

    % main loop
    for k = 1 : rank

        % initialize epoch
        epoch = 0;

        % initialize (x,y) with an optimal rank-one NMF of M
        [w, s, h] = svds(M, 1);
        ws = abs(w) * sqrt(s);
        hs = abs(h) * sqrt(s);
        W(:, k) = ws;
        H(k, :) = hs';

        % initialize Lagrangian variable lambda
        R = M - ws * hs';
        lambda = max(zeros(size(R)), -R);

        % inner loop
        while (optgap > options.tol_optgap) && (epoch < options.inner_max_epoch)

            % update ws and hs
            A = M - lambda;

            if options.Cnorm == 1
                % l_1 norm minimization
                ws = max(0, (wmedian(A, hs)));
                hs = max(0, (wmedian(A', ws)));

             elseif options.Cnorm == 2
                % l_2 norm minimization
                ws = max(0, A * hs);
                ws = ws / (max(ws) + 1e-16);
                hs = max(0, (A' * ws) / (ws' * ws));
            end

            % update lambda
            if sum(ws) ~= 0 && sum(hs) ~= 0
                R = M - ws * hs';
                W(:, k) = ws;
                H(k, :) = hs';
                lambda = max(0, lambda - R / ((epoch + 1) + 1));
            else
                lambda = lambda / 2;
                ws = W(:, k);
                hs = H(k, :)';
            end

            % measure elapsed time
            elapsed_time = toc(start_time);

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

            % update epoch
            epoch = epoch + 1;

            % store info
            % total iteration is computed as (k - 1) * options.inner_max_epoch + epoch
            W_rec = W(:, 1:k);
            H_rec = H(1:k, :);
            [infos, f_val, optgap] = store_nmf_info(V, W_rec, H_rec, [], options, infos, (k - 1) * options.inner_max_epoch + epoch, grad_calc_count, elapsed_time);

            % display infos
            if options.verbose > 2
                if ~mod(epoch, disp_freq)
                    fprintf('%s: k = %02d, Epoch = %04d, cost = %.16e, optgap = %.4e, time = %e\n', method_name, k, (k - 1) * options.inner_max_epoch + epoch, f_val, optgap, elapsed_time - prev_time);
                end
            end

        end

        M = max(0, M - ws * hs');

        % store info
        % total iteration is computed as (k - 1) * options.inner_max_epoch + epoch
        W_rec = W(:, 1:k);
        H_rec = H(1:k, :);
        [infos, f_val, optgap] = store_nmf_info(V, W_rec, H_rec, [], options, infos, (k - 1) * options.inner_max_epoch + epoch, grad_calc_count, elapsed_time);

        % display infos
        if options.verbose > 1
            if ~mod(epoch, disp_freq)
                fprintf('%s: k = %02d, Epoch = %04d, cost = %.16e, optgap = %.4e, time = %e\n', method_name, k, (k - 1) * options.inner_max_epoch + epoch, f_val, optgap, elapsed_time - prev_time);
            end
        end

        prev_time = elapsed_time;

    end

    if options.verbose > 0
        if optgap < options.tol_optgap
            fprintf('# Recursive-NMU: Optimality gap tolerance reached: f_val = %.4e < f_opt = %.4e (%.4e)\n', f_val, options.f_opt, options.tol_optgap);
        elseif (k - 1) * options.inner_max_epoch + epoch == rank * options.inner_max_epoch
            fprintf('# Recursive-NMU: Max epoch reached (%g).\n', rank * options.inner_max_epoch);
        end
    end

    x.W = W;
    x.H = H;

end

% WMEDIAN computes an optimal solution of
%
% min_x  || A - xy^T ||_1, y >= 0
%
% where A has dimension (m x n), x (m) and y (n),
% in O(mn log(n)) operations. Should be done in O(mn)...

function x = wmedian(A,y)

    % Reduce the problem for positive entries of y
    indi = y > 1e-16;
    A = A(:, indi);
    y = y(indi);
    [m, n] = size(A);
    A = A ./ repmat(y', m, 1);
    y = y / sum(y);

    % Sort rows of A, m*O(n log(n)) operations
    [As, Inds] = sort(A, 2);

    % Construct matrix of ordered weigths
    Y = y(Inds);

    % Extract the median
    actind = 1 : m;
    i = 1;
    sumY = zeros(m, 1);
    x = zeros(m, 1);
    while ~isempty(actind) % O(n) steps... * O(m) operations
        % sum the weitghs
        sumY(actind, :) = sumY(actind, :) + Y(actind, i);
        % check which weitgh >= 0
        supind = (sumY(actind, :) >= 0.5);
        % update corresponding x
        x(actind(supind)) = As(actind(supind), i);
        % only look reminding x to update
        actind = actind(~supind);
        i = i + 1;
    end
end


% Fig. 6.10   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
%

clear all;
%close all;
clf

num=10;
den=conv([1 0],[1 0.4 4]);
w=logspace(-1,2,100);
[m,p]=bode(num,den,w);

figure(1)
loglog(w,m);
xlabel('\omega (rad/sec)');
ylabel('Magnitude');
title('Fig. 6.10 Bode plot for a TF with a complex pole :(a) magnitude');
bodegrid;
%pause;
figure(2)
semilogx(w,p);
grid;
xlabel('\omega (rad/sec)');
ylabel('Phase');
title('Fig. 6.10 (b) phase');
bodegrid;


function [ft2] = mm22ft2(mm2)
% Convert area from square millimeters to square feet.
% Chad A. Greene 2012
ft2 = mm2*0.00001076391041671;


function dst_scalars=nsst_scalars(L,shear_f,lpfilt)
% Since the nonsubsampled descrite shearlet transform is not orthogonal
% this function computes the noise level scalars of the transform with
% assigned parameters.
%
% Inputs:
%
% L                      - size of image decomposition
%
% shear_f                - the cell array containing the shearing filters
%
%
% lpfilt                 - lpfilt is the filter to be used for the Laplacian
%                          Pyramid/ATrous decomposition using the codes
%                          written by Arthur Cunha
%
% Output:
%
% dst_scalars            - the cell array containing the scalars of
%                          estimated noise levels for a white Gaussian noise
%                          of standard deviation 1 transform coefficients
%                          using Monte Carlo method with one iteration.
%
% Code contributors: Glenn R. Easley, Demetrio Labate, and Wang-Q Lim.
% Copyright 2011 by Glenn R. Easley. All Rights Reserved.
%

noise=randn(L,L);
level=length(shear_f);

% LP decomposition
y_noise = atrousdec(noise,lpfilt,level);

dst_scalars=cell(1,level+1);
dst_noise=y_noise{1};
dst_scalars{1}=median(abs(dst_noise(:) - median(dst_noise(:))))/.6745;

for i=1:level,
    l=size(shear_f{i},3);
    for k=1:l,
        dst_noise=conv2p(shear_f{i}(:,:,k),y_noise{i+1});
        dst_scalars{i+1}(k)=median(abs(dst_noise(:) - median(dst_noise(:))))/.6745;
    end
end


function [inv_A, r, PR, PL] = invertProjection(A, epsilon)
% Inverts a general matrix A using the pseudoinverse
%
% USAGE:
%
%    [inv_A, r, PR, PL] = invertProjection(A, epsilon)
% INPUTS:
%    A:          general matrix
%    epsilon:    default = 1e-10
%
% OUTPUTS:
%    inv_A:      the pseudoinverse of `A`
%    r:          the rank of `A`
%    PR:        the projection matrix onto the `range(A)`
%    PL:        the projection matrix onto the `null(A')`

if nargin < 2
    epsilon = 1e-10;
end
[m, n] = size(A);

if 1
    %[U, S, V] = svd(A); % not working, uncommented line below - Lemmer
    %[U, S, V] = svds(A,min(size(A))); % Bugfix due to svd convergence problems
    [U, S, V] = svd(full(A),'econ'); %from Michael Saunders code
    r = sum(sum(abs(S) > epsilon));
    inv_S = diag(1 ./ S(abs(S) > epsilon));
    inv_A = V(:, 1:r) * inv_S(1:r, 1:r) * U(:, 1:r)';
    PR = U(:, 1:r)       * U(:, 1:r)';
    PL = U(:, (r+1):end) * U(:, (r+1):end)';

else
    %Michael Saunders code -TODO integrate this properly
    [U1,D1,V1,r] = subspaceSVD(A);
    PR=U1*U1';%projection matrix onto the range(A)
    PL=eye(m) - U1*U1';%projection matrix onto null(A')
    inv_A=pinv(A,1e-12);
end
    % Michael Saunders code
    % [U1,D1,V1,r] = subspaceSVD(A);
    % PR=U1*U1';%projection matrix onto the range(A)
    % PL=eye(m) - U1*U1';%projection matrix onto the null(A')
    % inv_A=pinv(A,1e-12);


function noise = noisevector(vlength,xcfunction,noisevariance)
% function noise = noisevector(vlength,xcfunction,noisevariance)
%
% returns a noise vector of 'vlength' samples
% with characteristics of the autocorrelation function that you specify
%   (must be same sampling rate as timeseries!)
%
% the vector is normalized such that the variance is what you specify
% and the mean is zero
%
% vlength: vector length
% xcfunction: any cross-correlation (i.e., noise autocorrelation) function
%   - also called a periodogram
%   - a 1/f function is generally good for fMRI data
% noisevariance: desired variance of noise
%
% 2/11/01 Tor Wager

if isempty(xcfunction), xcfunction = [1 0];,end

% define series of random "shocks".  Last one is time 1, paramount one is time 2, etc.
%       pad with zeros, because 1st shock has no history to influence it.
xclength = size(xcfunction,2);
a = randn(1,vlength); a(end + 1:end + xclength) = 0;
% the noise is the series of shocks multiplied by the autocorrelation function
% so the current value of the system = weighted sum of past shocks, up to size of autocorr function
for i = 1:vlength
    noise(i) = dot(a(end-i-xclength+1:end-i),xcfunction');
end

% make sure that the variance is one and the mean is zero

noise = noise - mean(noise);

noise = noise / sqrt(var(noise));

% now make the variance match your estimate of the variance

noise = noise * sqrt(noisevariance);

%disp(['Noise variance is ' num2str(var(noise))])
%figure;plot(noise)

return


function h=demirel(im,thr,T)
%h=demirel(im,thr);
%im is an input image, thr is a threshold between 0-1, T is the thickness
%of the line to indicate the edge.
%h is an uint8 balck and white image with values of 0 and 255.
%This programme has been written by me, G. Anbarjafari (Shahab) months ago
%but finalized today 17-11-2008.
%(c) Demirel and Anbarjafari - 2008

[sx,sy,sz]=size(im);

if sz~=1
    im1=not(im2bw(rgb2gray(im),thr));
else
    im1=not(im2bw(im,thr));
end

SZ=2*T+1;
X=zeros(SZ,SZ);
X((SZ+1)/2,:)=ones(1,SZ);
X(:,(SZ+1)/2)=ones(SZ,1);
X((SZ+1)/2,(SZ+1)/2)=2;
Q = filter2(X,im1);
Q([find(Q<1)])=0;
Q([find(Q>0)])=1;

h=uint8(abs(double(Q)-double(im1))*255);


function g = p03_g ( n, x )

%*****************************************************************************80
%
%% P03_G evaluates the gradient for problem 3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real G(N), the gradient of the objective function.
%
  y = p03_yvec ( );

  g = zeros ( n, 1 );

  for i = 1 : 15

    d1 = 0.5 * ( i - 1 );
    d2 = 3.5 - d1 - x(3);
    arg = - 0.5 * x(2) * d2 * d2;
    t = x(1) * exp ( arg ) - y(i);

    g(1) = g(1) + 2.0 * exp ( arg ) * t;
    g(2) = g(2) - x(1) * exp ( arg ) * t * d2 * d2;
    g(3) = g(3) + 2.0 * x(1) * x(2) * exp ( arg ) * t * d2;

  end

  return
end


function kern = matern32KernParamInit(kern)

% MATERN32KERNPARAMINIT MATERN32 kernel parameter initialisation.
% The Matern class of kernels is a wide class with different
% degrees of freedom parameters. This is the specific case where nu
% = 3/2.
%
% Given
% r = sqrt((x_i - x_j)'*(x_i - x_j))
%
% We have
% k(x_i, x_j) = sigma2*(1+sqrt(3)*r/l)*exp(-sqrt(3)*r/l)
%
% The parameters are sigma2, the process variance (kern.variance),
% and l, the length scale (kern.lengthScale).
% FORMAT
% DESC initialises the matern kernel with nu=3/2
%  kernel structure with some default parameters.
% ARG kern : the kernel structure which requires initialisation.
% RETURN kern : the kernel structure with the default parameters placed in.
%
% SEEALSO : kernCreate, kernParamInit
%
% COPYRIGHT : Neil D. Lawrence, 2006

% KERN

kern.variance = 1;
kern.lengthScale = 1;
kern.nParams = 2;

kern.transforms.index = [1 2];
kern.transforms.type = optimiDefaultConstraint('positive');

kern.isStationary = true;


%% Author: epokh
%% Website: www.epokh.org/drupy
%% This software is under GPL

clc;
close all;
%%This example shows a trajectory planning example for the
%%antropomorphic arm

%%Link length
a2=1;
a3=1;

%%End effector initial point
%% by inverse kinematics we have the joint starting variables
[theta1s,theta2s,theta3s]=inverseAntro(1,0.5,0.3,a2,a3);

%%End effector final point
%% by inverse kinematics we have the joint ends variables
[theta1e,theta2e,theta3e]=inverseAntro(0.1,0.5,0.3,a2,a3);

%%3 joint variables: 3 trajectories
%% We want the movement be completed in 6 seconds
tstart=0;
tend=6;
time=[tstart tend];

%
% %%Check the forward kinematics and the multiple inverse solutions
figure(1);
T01=DHmatrix(theta1s(1),0,0,45);
T12=DHmatrix(theta2s(1),0,a2,0);
T23=DHmatrix(theta3s(1),0,a3,0);
Tuh1=T01*T12*T23;
figure(1);
hold on;
plotT(Tuh1);
T01=DHmatrix(theta1s(2),0,0,45);
T12=DHmatrix(theta2s(2),0,a2,0);
T23=DHmatrix(theta3s(2),0,a3,0);
Tuh2=T01*T12*T23;
plotT(Tuh2);

%%Once we have the kinematics and paths we can plot the movements!
%%Use a spline interpolation

pp1 = spline(time,[0 theta1s(1) theta1e(1) 0]);
pp2 = spline(time,[0 theta2s(1) theta2e(1) 0]);
pp3 = spline(time,[0 theta3s(1) theta3e(1) 0]);
time=linspace(tstart,tend);
figure(2);
subplot(3,1,1);
title('Position Theta1');
plot(time,fnval(pp1,time),'b');
xlabel('Time');
ylabel('Theta1');
subplot(3,1,2);
title('Position Theta2');
plot(time,fnval(pp2,time),'b');
xlabel('Time');
ylabel('Theta2');
subplot(3,1,3);
title('Position Theta3');
plot(time,fnval(pp3,time),'b');
xlabel('Time');
ylabel('Theta3');

figure(3);
for k=1:1:length(time)
clf;
theta1=fnval(pp1,time(k));
theta2=fnval(pp2,time(k));
theta3=fnval(pp3,time(k));
T01=DHmatrix(theta1,0,0,45);
T12=DHmatrix(theta2,0,a2,0);
T23=DHmatrix(theta3,0,a3,0);
Tuh1=T01*T12*T23;
plotT(T01);
plotT2(T01,T01*T12);
plotT2(T01*T12,T01*T12*T23);
pause(0.1);
title('Arm trajectory');
end


% fastregress - perform fast regression and return p-value
%
% Usage:
% [ypred, alpha, rsq, B] = myregress(x, y, plotflag);
%
% Inputs
%  y - y values
%  x - x values
%  plotflag - [0|1] plot regression
%
% Outputs
%  ypred - y prediction
%  alpha - significance level
%  R^2   - r square
%  slope - slope of the fit
%
% Arnaud Delorme, 25 Feb 2003

function [ypred, alpha, rsq, B, BINT] = fastregress(x, y, plotflag)

    if nargin < 1
        help fastregress; return;
    end;

    % this part is useless but still works
    %B=polyfit(x, y, 1);         % B is the slope
    %ypred = polyval(B,x);       % predictions
    %dev = y - mean(y);          % deviations - measure of spread
    %SST = sum(dev.^2);          % total variation to be accounted for
    %resid = y - ypred;          % residuals - measure of mismatch
    %SSE = sum(resid.^2);        % variation NOT accounted for
    %rsq = 1 - SSE/SST;          % percent of error explained
    % see the page  http://www.facstaff.bucknell.edu/maneval/help211/fitting.html

    [B,BINT,R,RINT,STATS] = regress(y(:), [ones(length(x),1) x(:)]);
    alpha = STATS(3);
    rsq   = STATS(1);

    %note that we also have
    %ypred =  [ones(size(x,2),1) x(:)]*B;
    ypred =  x*B(2) + B(1);

    % B(1) contain the offset, B(2) the slope
    B = B(2);

    if nargin > 2
        hold on;
        [ynew tmp] = sort(ypred);
        xnew       = x(tmp);
        plot(xnew, ynew, 'r');
        legend(sprintf('R^2=%f', rsq), sprintf('p  =%f', alpha));
    end;


clear; clc;
%% desired signal
% n = (1:1000)';
%#codegen
% s = sin(0.075*pi*n);

%% noise signal
% v = 0.8*randn(1000,1); % Random noise part.
% ar = [1,1/2];          % Autoregression coefficients.
% v1 = filter(1,ar,v);   % Noise signal. Applies a 1-D digital filter.

%% primary input (noise corrupted signal)
% x = s + v1;
[x,fs1] = audioread('x14.wav');

%% reference input
% ma = [1, -0.8, 0.4 , -0.2];
% v2 = filter(ma,1,v);
[v2,fs1] = audioread('x15.wav');

%% adaptive filter
% initialization
L = 512;
% lms = dsp.LMSFilter(L,'Method','LMS');
nlms = dsp.LMSFilter(L,'Method','Normalized LMS','LeakageFactor',1,'AdaptInputPort',false,...
'WeightsResetInputPort',false,'WeightsOutput','Last');
% filter step size
% [mumaxlms,mumaxmselms]   = maxstep(lms,x);
[mumaxnlms,mumaxmsenlms] = maxstep(nlms,x); % maxstep function of dsp.LMSFilter
% lms.StepSize  = mumaxmselms/30;
nlms.StepSize = mumaxmsenlms/6;

%% filter with the adaptive filter
% [ylms,elms,wlms] = lms(v2,x);
[ynlms,enlms,wnlms] = nlms(v2,x);

%% compute the optimal solution (FIR Wiener filter)
% bw = firwiener(L-1,v2,x); % Optimal FIR Wiener filter
% yw = filter(bw,1,v2);   % Estimate of x using Wiener filter
% ew = x - yw;            % Estimate of actual sinusoid

%% plot
% primary input
% plot(n(900:end),x(900:end),'k:')
% xlabel('Time index (n)');
% ylabel('Amplitude');
plot([1:length(x)], x, 'm:')
xlabel('Time index (n)');
ylabel('Amplitude');

% denoised result
hold on;
% plot(n(900:end),[ew(900:end), elms(900:end),enlms(900:end)]);
% legend('Wiener filter denoised sinusoid',...
%     'LMS denoised sinusoid','NLMS denoised sinusoid');
% xlabel('Time index (n)');
% ylabel('Amplitude');
plot([1:length(x)], enlms);
xlabel('Time index (n)');
ylabel('Amplitude');
legend('primary input','denoised result');
hold off;

%% reset nlms
reset(nlms);

%% convergence investigation through learning curve
% M = 10; % Decimation factor
% msenlms = msesim(nlms,v2,x,M);
% figure;
% plot(1:M:x(end),msenlms)
% legend('LMS learning curve','NLMS learning curve')
% xlabel('Time index (n)');
% ylabel('MSE');

%% theorectical learning curves
% reset(nlms);
% figure;
% [mmselms,emselms,meanwlms,pmselms] = msepred(nlms,v2,x,M);
% plot(1:M:x(end),[mmselms*ones(500,1),emselms*ones(500,1),...
%         pmselms,mselms])
% legend('MMSE','EMSE','predicted LMS learning curve',...
%     'LMS learning curve')
% xlabel('Time index (n)');
% ylabel('MSE');


function traj = simulateCannon(init,param)

v0 = init.speed;
th0 = init.angle;

c = param.c;  %Quadratic drag coefficient
nGrid = param.nGrid;

% Set up initial conditions for ode45
x0 = 0;  y0 = 0;  %Start at the origin
dx0 = v0*cos(th0);
dy0 = v0*sin(th0);
if dy0 < 0, error('Cannot point cannon through ground! sin(th0) > 0 required.'); end;

% Set up arguments for ode45
userFun = @(t,z)cannonDynamics(t,z,c);  %Dynamics function
tSpan = [0,100];  %Never plan on reaching final time
z0 = [x0;y0;dx0;dy0];  %Initial condition
options = odeset('Events',@groundEvent,'Vectorized','on');

% Run a simulation
sol = ode45(userFun, tSpan, z0, options);

% Extract the solution on uniform grid:
traj.t = linspace(sol.x(1), sol.x(end), nGrid);
z = deval(sol,traj.t);
traj.x = z(1,:);
traj.y = z(2,:);
traj.dx = z(3,:);
traj.dy = z(4,:);

end


function [cvx_optval,P,q,r,X,lambda] = cheb(A,b,Sigma) %#ok

% Computes Chebyshev lower bounds on probability vectors
%
% Calculates a lower bound on the probability that a random vector
% x with mean zero and covariance Sigma satisfies A x <= b
%
% Sigma must be positive definite
%
% output arguments:
% - prob: lower bound on probability
% - P,q,r: x'*P*x + 2*q'*x + r is a quadratic function
%   that majorizes the 0-1 indicator function of the complement
%   of the polyhedron,
% - X, lambda:  a discrete distribution with mean zero, covariance
%   Sigma and Prob(X not in C)  >= 1-prob

%
% maximize  1 - Tr Sigma*P - r
% s.t.      [ P  q     ]             [ 0      a_i/2 ]
%           [ q' r - 1 ] >= tau(i) * [ a_i'/2  -b_i ], i=1,...,m
%           taui >= 0
%           [ P q  ]
%           [ q' r ] >= 0
%
% variables P in Sn, q in Rn, r in R
%

[ m, n ] = size( A ); %#ok
cvx_begin sdp quiet
    variable P(n,n) symmetric
    variables q(n) r tau(m)
    dual variables Z{m}
    maximize( 1 - trace( Sigma * P ) - r )
    subject to
        for i = 1 : m,
            qadj = q - 0.5 * tau(i) * A(i,:)';
            radj = r - 1 + tau(i) * b(i);
            [ P, qadj ; qadj', radj ] >= 0 : Z{i}; %#ok
        end
        [ P, q ; q', r ] >= 0; %#ok
        tau >= 0; %#ok
cvx_end

if nargout < 4,
    return
end

X = [];
lambda = [];
for i=1:m
   Zi = Z{i};
   if (abs(Zi(3,3)) > 1e-4)
      lambda = [lambda; Zi(3,3)]; %#ok
      X = [X Zi(1:2,3)/Zi(3,3)]; %#ok
   end;
end;
mu = 1-sum(lambda);
if (mu>1e-5)
   w = (-X*lambda)/mu;
   W = (Sigma - X*diag(lambda)*X')/mu;
   [v,d] = eig(W-w*w');
   d = diag(d);
   s = sum(d>1e-5);
   if (d(1) > 1e-5)
      X = [X w+sqrt(s)*sqrt(d(1))*v(:,1) ...
            w-sqrt(s)*sqrt(d(1))*v(:,1)];
      lambda = [lambda; mu/(2*s); mu/(2*s)];
   elseif (d(2) > 1e-5)
      X = [X w+sqrt(s)*sqrt(d(2))*v(:,2) ...
            w-sqrt(s)*sqrt(d(2))*v(:,2)];
      lambda = [lambda; mu/(2*s); mu/(2*s)];
   else
      X = [X w];
      lambda = [lambda; mu];
   end;
end;


function [tfr,t,f] = tfrbud(x,t,N,g,h,sigma,trace);
%TFRBUD	Butterworth time-frequency distribution.
%	[TFR,T,F]=TFRBUD(X,T,N,G,H,SIGMA,TRACE) computes the Butterworth
%	distribution of a discrete-time signal X, or the
%	cross Butterworth representation between two signals.
%
%	X     : signal if auto-BUD, or [X1,X2] if cross-BUD.
%	T     : time instant(s)          (default : 1:length(X)).
%	N     : number of frequency bins (default : length(X)).
%	G     : time smoothing window, G(0) being forced to 1.
%	                                 (default : Hamming(N/4)).
%	H     : frequency smoothing window, H(0) being forced to 1.
%	                                 (default : Hamming(N/4)).
%	SIGMA : kernel width             (default : 1).
%	TRACE : if nonzero, the progression of the algorithm is shown
%                                        (default : 0).
%	TFR   : time-frequency representation. When called without
%               output arguments, TFRBUD runs TFRQVIEW.
%	F     : vector of normalized frequencies.
%
%	Example :
%	 sig=fmlin(128,0.05,0.3)+fmlin(128,0.15,0.4);
%	 g=tftb_window(9,'Kaiser'); h=tftb_window(27,'Kaiser');
%	 t=1:128; tfrbud(sig,t,128,g,h,3.6,1);
%
%	See also all the time-frequency representations listed in
%	 the file CONTENTS (TFR*)

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

if (nargin == 0),
 error('At least 1 parameter required');
end;
[xrow,xcol] = size(x);
if (xcol==0)|(xcol>2),
 error('X must have one or two columns');
end

if (nargin <= 2),
 N=xrow;
elseif (N<0),
 error('N must be greater than zero');
elseif (2^nextpow2(N)~=N),
 fprintf('For a faster computation, N should be a power of two\n');
end;

hlength=floor(N/4); hlength=hlength+1-rem(hlength,2);
glength=floor(N/10);glength=glength+1-rem(glength,2);

if (nargin == 1),
 t=1:xrow; g = tftb_window(glength); h = tftb_window(hlength); sigma = 1.0; trace = 0;
elseif (nargin == 2)|(nargin == 3),
 g = tftb_window(glength); h = tftb_window(hlength); sigma = 1.0; trace = 0;
elseif (nargin == 4),
 h = tftb_window(hlength); sigma = 1.0; trace = 0;
elseif (nargin == 5),
 sigma = 1.0; trace = 0;
elseif (nargin == 6),
 trace = 0;
end;

[trow,tcol] = size(t);
if (trow~=1),
 error('t must only have one row');
end;

[grow,gcol]=size(g); Lg=(grow-1)/2;
if (gcol~=1)|(rem(grow,2)==0),
 error('G must be a smoothing window with odd length');
end;

[hrow,hcol]=size(h); Lh=(hrow-1)/2; h=h/h(Lh+1);
if (hcol~=1)|(rem(hrow,2)==0),
 error('H must be a smoothing window with odd length');
end;

if (sigma<=0.0),
 error('SIGMA must be strictly positive');
end;

taumax = min([round(N/2),Lh]); tau = 1:taumax; points = -Lg:Lg;
BudKer = exp(-kron( abs(points.'), 1.0 ./ (2.0*tau/sqrt(sigma))));
BudKer = diag(g) * BudKer;

tfr= zeros (N,tcol) ;
if trace, disp('Butterworth distribution'); end;
for icol=1:tcol,
 ti= t(icol); taumax=min([ti+Lg-1,xrow-ti+Lg,round(N/2)-1,Lh]);
 if trace, disprog(icol,tcol,10); end;
 tfr(1,icol)= x(ti,1) .* conj(x(ti,xcol));

 for tau=1:taumax,
  points= -min([Lg,xrow-ti-tau]):min([Lg,ti-tau-1]);
  g2 = BudKer(Lg+1+points,tau); g2=g2/sum(g2);
  R=sum(g2 .* x(ti+tau-points,1) .* conj(x(ti-tau-points,xcol)));
  tfr(  1+tau,icol)=h(Lh+tau+1)*R;
  R=sum(g2 .* x(ti-tau-points,1) .* conj(x(ti+tau-points,xcol)));
  tfr(N+1-tau,icol)=h(Lh-tau+1)*R;
 end;

 tau=round(N/2);
 if (ti<=xrow-tau)&(ti>=tau+1)&(tau<=Lh),
  points= -min([Lg,xrow-ti-tau]):min([Lg,ti-tau-1]);
  g2 = BudKer(Lg+1+points,tau); g2=g2/sum(g2);
  tfr(tau+1,icol) = 0.5 * ...
   (h(Lh+tau+1)*sum(g2 .* x(ti+tau-points,1) .* conj(x(ti-tau-points,xcol)))+...
    h(Lh-tau+1)*sum(g2 .* x(ti-tau-points,1) .* conj(x(ti+tau-points,xcol))));
 end;
end;

clear BudKer;

if trace, fprintf('\n'); end;
tfr= fft(tfr);
if (xcol==1), tfr=real(tfr); end ;

if (nargout==0),
 tfrqview(tfr,x,t,'tfrbud',g,h,sigma);
elseif (nargout==3),
 f=(0.5*(0:N-1)/N)';
end;


function res = SumKL(p,K,L)
%SUMKL        Summation 'as if' computed in K-fold precision and stored in L results
%
%   res = SumKL(p,K,L)
%
%On return, sum(res) approximates sum(p) with accuracy as if computed
%  in K-fold precision, where res comprises of L elements.
%  Default for L is 1.
%
%Implements algorithm SumKL from
%  S.M. Rump: Inversion of extremely ill-conditioned matrices in floating-point,
%      Japan J. Indust. Appl. Math. (JJIAM), 26:249-277, 2009.
%
%Reference implementation! Slow due to interpretation!
%

% written  06/23/08     S.M. Rump
% modified 05/09/09     S.M. Rump  rounding to nearest
%

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

  if nargin==2
    L = 1;
  end

  n = length(p);
  for i=1:K-L
    p = VecSum(p);
  end
  res = zeros(1,L);
  for k=0:L-2
    p(1:n-k) = VecSum(p(1:n-k));
    res(k+1) = p(n-k);
  end
  res(L) = sum(p(1:n-L+1));

  if rndold
    setround(rndold)
  end


% Compute the log intensity for the inverse link function g(f) = 1/(1+exp(-f)).
%
% The function is used in GLM likelihoods such as likPoisson, likGamma, likBeta
% and likInvGauss.
%
% Copyright (c) by Hannes Nickisch, 2013-10-16.

function varargout = glm_invlink_logit(f)
  varargout = cell(nargout, 1);  % allocate the right number of output arguments
  [varargout{:}] = glm_invlink_logistic(f);
  if nargout>0
    elg = exp(varargout{1});
    varargout{1} = f - elg;
    if nargout>1
      dlg = varargout{2};
      varargout{2} = 1 - elg.*dlg;
      if nargout>2
        d2lg = varargout{3};
        varargout{3} = -elg.*(dlg.^2+d2lg);
        if nargout>3
          d3lg = varargout{4};
          varargout{4} = -elg.*(dlg.^3+3*d2lg.*dlg+d3lg);
        end
      end
    end
  end


function vCubo = homo2cubo(vHomo)
% homochoric to cubochoric coordinates
%
% Transforms homochoric coordinates of quaternions into cubochoric
% coordinates. This mpas the ball of radius (3*pi/4)^(1/3)) to the cube
% with edge length pi^(2/3).
%
% Input
%  xyz - homochoric coordinates (x,y,z) of N points of the ball
%
% Output
%  XYZ - cubochoric coordinates (X,Y,Z) of N points of the cube
%

% the actual mapping is only defined on one pyramid Pz (z>=abs(x),z>=abs(y))
% map other points by:
%  1. transform coordinates, so that we get a point of Pz
%  2. map the point
%  3. apply the inverse transformation

% for each point find out, which pyramid it lies in (1,2,3,4,5,6)
rId = cuboRegionId(vHomo);

% define permutaions (and inverse ones) for each region (pyramid)
permRegion  = [2 3 1; 3 2 1; 1 3 2; 3 1 2; 1 2 3; 2 1 3];
ipermRegion = [3 1 2; 3 2 1; 1 3 2; 2 3 1; 1 2 3; 2 1 3];

% apply the permutation on each row
vHomo = vHomo(sub2ind(size(vHomo), ...
  (1:size(vHomo,1)).' * [1 1 1] ,permRegion(rId,:)));

% map each point
p = sqrt(sum(vHomo.^2,2));
D = sqrt(2 ./ (1 + abs(vHomo(:,3)) ./ p));
E = sqrt(2 * vHomo(:,1).^2 + vHomo(:,2).^2);
F = sqrt(abs(vHomo(:,1)) + E);
G = sign(vHomo(:,1));
H = sqrt(pi / 12);
I = (6 / pi)^(1/6);
K = D .* sqrt(E) .* F .* I;

vCubo(:,1) = K .* G .* H;
vCubo(:,2) = K ./ H .* (G .* atan(vHomo(:,2)./vHomo(:,1)) - atan(vHomo(:,2)./E));
vCubo(:,3) = sign(vHomo(:,3)) .* p ./ I^2;

% overwrite points with division by zero (occurs along the axes)
isNull = (vHomo(:,1)==0);
vCubo(isNull,:) = vHomo(isNull,:) / (6/pi)^(1/3);

% apply the inverse permutations
vCubo = vCubo(sub2ind(size(vCubo), ...
  (1:size(vCubo,1)).' * [1 1 1] , ipermRegion(rId,:)));

end


function x = ncc_abscissas_ab ( a, b, n )

%*****************************************************************************80
%
%% NCC_ABSCISSAS_AB computes the Newton Cotes Closed abscissas for [A,B].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 July 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the endpoints of the interval.
%
%    Input, integer N, the order of the rule.
%
%    Output, real X(N), the abscissas.
%
  if ( n == 1 )
    x(1) = 0.5 * ( b + a );
    return
  end

  for i = 1 : n
    x(i) = ( ( n - i     ) * a   ...
           + (     i - 1 ) * b ) ...
           / ( n     - 1 );
  end

  return
end


function [node,elem]=meshgrid6(varargin)
%
% [node,elem]=meshgrid6(v1,v2,v3,...)
%
% mesh an ND rectangular lattice by splitting
% each hypercube into 6 tetrahedra
%
% author: John D'Errico
% URL: http://www.mathworks.com/matlabcentral/newsreader/view_thread/107191
% modified by Qianqian Fang (q.fang at neu.edu)
%
% input:
%    v1,v2,v3,... - numeric vectors defining the lattice in
%                   each dimension.
%                   Each vector must be of length >= 1
%
% output:
%    node - factorial lattice created from (v1,v2,v3,...)
%           Each row of this array is one node in the lattice
%    elem - integer array defining simplexes as references to
%           rows of "node".
%
% example:
%     [node,elem]=meshgrid6(0:5,0:6,0:4);
%     plotmesh(node,elem);
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

% dimension of the lattice
n = length(varargin);

% create a single n-d hypercube
% list of node of the cube itself
vhc=('1'==dec2bin(0:(2^n-1)));
% permutations of the integers 1:n
p=perms(1:n);
nt=factorial(n);
thc=zeros(nt,n+1);
for i=1:nt
  thc(i,:)=find(all(diff(vhc(:,p(i,:)),[],2)>=0,2))';
end

% build the complete lattice
nodecount = cellfun('length',varargin);
if any(nodecount<2)
  error 'Each dimension must be of size 2 or more.'
end
node = lattice(varargin{:});

% unrolled index into each hyper-rectangle in the lattice
ind = cell(1,n);
for i=1:n
ind{i} = 0:(nodecount(i)-2);
end
ind = lattice(ind{:});
k = cumprod([1,nodecount(1:(end-1))]);
ind = 1+ind*k';
nind = length(ind);

offset=vhc*k';
elem=zeros(nt*nind,n+1);
L=(1:nind)';
for i=1:nt
  elem(L,:)=repmat(ind,1,n+1)+repmat(offset(thc(i,:))',nind,1);
  L=L+nind;
end

if(n==2 || n==3)
  elem=meshreorient(node,elem);
end

% ======== subfunction ========
function g = lattice(varargin)
% generate a factorial lattice in n variables
n=nargin;
sizes = cellfun('length',varargin);
c=cell(1,n);
[c{1:n}]=ndgrid(varargin{:});
g=zeros(prod(sizes),n);
for i=1:n
g(:,i)=c{i}(:);
end


function g = p01_g ( n, x )

%*****************************************************************************80
%
%% P01_G evaluates the gradient for problem 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real G(N), the gradient of the objective function.
%
  g = zeros ( n, 1 );

  th = p01_th ( x );

  r = sqrt ( x(1) * x(1) + x(2) * x(2) );
  t = x(3) - 10.0 * th;
  s1 = 5.0 * t / ( pi * r * r );

  g(1) = 200.0 * ( x(1) - x(1) / r + x(2) * s1 );
  g(2) = 200.0 * ( x(2) - x(2) / r - x(1) * s1 );
  g(3) = 2.0 * ( 100.0 * t + x(3) );

  return
end


function e = boundedges ( p, t )

%*****************************************************************************80
%
%% BOUNDEDGES finds the boundary edges in a triangular mesh.
%
%  Discussion:
%
%    You may need this routine if you need to enforce boundary
%    conditions in a PDE.
%
%    The 3D version of this code is called SURFTRI.
%
%  Licensing:
%
%    (C) 2004 Per-Olof Persson.
%    See COPYRIGHT.TXT for details.
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, real P(NP,2), the coordinates of a set of nodes.
%
%    Input, integer T(NT,1:3), a list of the nodes which make up each triangle
%    of a triangulation of the nodes in P.
%
%    Output, integer E(*,*), ?
%

%
%  Form all edges, non-duplicates are boundary edges
%
  edges = [t(:,[1,2]);
           t(:,[1,3]);
           t(:,[2,3])];

  node3 = [t(:,3);t(:,2);t(:,1)];
  edges = sort(edges,2);
  [foo,ix,jx] = unique(edges,'rows');
  vec = histc(jx,1:max(jx));
  qx = find(vec==1);
  e = edges(ix(qx),:);
  node3 = node3(ix(qx));
%
%  Orientation
%
  v1 = p(e(:,2),:)-p(e(:,1),:);
  v2 = p(node3,:)-p(e(:,1),:);
  ix = find(v1(:,1).*v2(:,2)-v1(:,2).*v2(:,1)>0);
  e(ix,[1,2]) = e(ix,[2,1]);

  return
end


function c = wavecdf97(x, nlevel)
%WAVECDF97: Multi-level discrete 2-D wavelet transform
%with the Cohen-Daubechies-Feauveau (CDF) 9/7 wavelet.
%
% c = wavecdf97(x, nlevel) does the follows according to the value of
%   nlevel:
%   nlevel > 0:   decomposes 2-dimension matrix x up to nlevel level;
%   nlevel < 0:   does the inverse transform to nlevel level;
%   nlevel = 0:   sets c equal to x;
%   omitted:      does the same as nlevel=5.
%
% The boundary handling method is symmetric extension.
%
% x may be of any size; it need not have size divisible by 2^L.
%   For example, if x has length 9, one stage of decomposition
%   produces a lowpass subband of length 5 and a highpass subband
%   of length 4.  Transforms of any length have perfect
%   reconstruction (exact inversion).
%   NOTE: the 5 lines above are quoted directly form [3].
%
% If nlevel is so large that the approximation coefficients become
%   a 1-D array, any further decomposition will be performed as for 1-D
%   decomposition until the approximation coefficients be a scale number.
%
% Lifting algorithm is not used here; we use subband filters directly.
%   Lifting algorithm and spline 5/3 wavelets and other jpeg2000 related
%   codes will be available soon.
%
% Example:
%   Y = wavecdf97(X, 5);    % Decompose image X up to 5 level
%   R = wavecdf97(Y, -5);   % Reconstruct from Y
%
% You can test wavecdf97.m with the following lines:
%   % get a 2-D uint8 image
%   x=imread('E:\study\jpeg2000\images\lena.tif');
%   % decompose
%   y=wavecdf97(x,2);
%   % show decomposed result
%   figure;imshow(mat2gray(y));
%   % reconstruct without change of anything
%   ix=wavecdf97(y,-2);
%   % show and compare the original and reconstructed images
%   figure;subplot(1,2,1);imshow(x);subplot(1,2,2);imshow(uint8(ix));
%   % look at the MSE difference
%   sum(sum((double(x)-ix).^2))/numel(x)
%
% Reference:
%   [1] D.S.Taubman et al., JPEC2000 Image Compression: F. S. & P.,
%       Chinese Edition, formula 10.6-10.9 in section 10.3.1
%       and formula 10.13 in section 10.4.1.
%   [2] R.C.Gonzalez et al., Digital Image Processing Using MATLAB,
%       Chinese Edition, function wavefast in section 7.2.2.
%   [3] Pascal Getreuer, waveletcdf97.m from Matlab file Exchange website
%   [4] Matlab files: biorwavf.m, wavdec2.m, wawrec2.m, etc.
%
% Contact information:
%   Email/MSN messenger:  wangthth@hotmail.com
%
% Tianhui Wang at Beijing, China,   July, 2006
%                  Last Revision:   Aug 5, 2006

%---------------------- input arguments checking  ----------------------%
error(nargchk(1,2,nargin));
if nargin == 1
    nlevel = 5; % default level
end
% check x
if ~isreal(x) || ~isnumeric(x) || (ndims(x) > 2)
    error('WAVELIFT:InArgErr', ['The first argument must' ...
        ' be a real, numeric 2-D or 1-D matrix.']);
end
if isinteger(x)
    x = double(x);
end
% check nlevel
if ~isreal(nlevel) || ~isnumeric(nlevel) || round(nlevel)~=nlevel
    error('WAVELIFT:InArgErr', ['The 2nd argument shall be ' ...
        'a real and numeric integer.']);
end
%---------------- forming low-pass and high-pass filters ---------------%
% CDF 9/7 filters: decomposition low-pass lp and high-pass hp
%                  reconstruction low-pass lpr and high-pass hpr
% The filter coefficients have several forms.
% What D.S.Taubman et al. suggest in [1] are used here:
lp = [.026748757411 -.016864118443 -.078223266529 .266864118443];
lp = [lp .602949018236 fliplr(lp)];
hp = [.045635881557 -.028771763114 -.295635881557];
hp = [hp .557543526229 fliplr(hp)];
lpr = hp .* [-1 1 -1 1 -1 1 -1] * 2;
hpr = lp .* [1 -1 1 -1 1 -1 1 -1 1] * 2;
% Matlab 'bior4.4' use the varied version (see Matlab's biorwavf.m):
%  lp=lp*sqrt(2);hp=hp*(-sqrt(2));lpr=lpr*(1/sqrt(2));hpr=hpr*(-1/sqrt(2));
% P.Getreuer's waveletcdf97.m [3] alters the Taubman's version as follows:
%  lp=lp*sqrt(2);hp=hp*sqrt(2);lpr=lpr*(1/sqrt(2));hpr=hpr*(1/sqrt(2));
% while R.C.Gonzalez et al in [2] alter the Taubman's version as follows:
%  lp=lp;hp=hp*(-2);lpr=lpr;hpr=hpr*(-1/2);
%----------------  remain unchanged when nlevel = 0  -------------------%
if nlevel == 0
    c = x;
%--------------------  decomposition,  if nlevel < 0  ------------------%
elseif nlevel > 0
    c = zeros(size(x));
    x = double(x);
    for i = 1:nlevel
        % [ll, hl; lh, hh]: 1-level FWT for x
        temp = symconv2(x, hp, 'col');    % high filtering
        temp = temp(2:2:end, :);          % down sampling
        hh = symconv2(temp, hp, 'row');   % high filtering
        hh = hh(:, 2:2:end);              % down sampling
        lh = symconv2(temp, lp, 'row');   % low filtering
        lh = lh(:, 1:2:end);              % down sampling

        temp = symconv2(x, lp, 'col');    % low filtering
        temp = temp(1:2:end, :);          % down sampling
        hl = symconv2(temp, hp, 'row');   % high filtering
        hl = hl(:, 2:2:end);              % down sampling
        ll = symconv2(temp, lp, 'row');   % low filtering
        ll = ll(:, 1:2:end);              % down sampling
        % update coefficient matrix
        c(1:size(x,1), 1:size(x,2)) = [ll, hl; lh, hh];
        % replace x with ll for next level FWT
        x = ll;
        % give a warning if nlevel is too large
        if size(x,1)<=1 && size(x,2)<=1 && i~=nlevel
            warning('WAVECDF97:DegradeInput', ['Only decompose to ' ...
                num2str(i) '-level instead of ' num2str(nlevel) ...
                ', \nas the approximation coefficients at ' num2str(i) ...
                '-level has row or/and column of length 1.']);
            break
        end
    end
%--------------------  reconstruction,  if nlevel < 0  -----------------%
else
    sx = size(x);
    % find reconstruction level
    nl = -nlevel;
    while sx(1)/2^nl<=1/2 && sx(2)/2^nl<=1/2,  nl = nl-1;  end
    if nl ~= -nlevel
        warning('WAVECDF97:DegradeInput', ['Only reconstruct to ' ...
            num2str(nl) '-level instead of ' num2str(-nlevel) ...
            ', \nas the approximation coefficients at ' num2str(nl) ...
            '-level has row or/and column of length 1.']);
    end
    % nl-level reconstruction
    for i = 1:nl
        % find the target ll hl lh hh blocks
        sLL = ceil(sx/2^(nl-i+1));
        sConstructed = ceil(sx/2^(nl-i));
        sHH = sConstructed - sLL;
        lrow = sConstructed(1); lcol = sConstructed(2);

        ll = x(1:sLL(1), 1:sLL(2));
        hl = x(1:sLL(1), sLL(2)+1:sLL(2)+sHH(2));
        lh = x(sLL(1)+1:sLL(1)+sHH(1), 1:sLL(2));
        hh = x(sLL(1)+1:sLL(1)+sHH(1), sLL(2)+1:sLL(2)+sHH(2));

        % upsample rows and low filter
        temp = zeros(sLL(1), lcol); temp(:, 1:2:end) = ll;
        ll = symconv2(temp, lpr, 'row');
        % upsample rows and high filter
        temp = zeros(sLL(1), lcol); temp(:, 2:2:end) = hl;
        hl = symconv2(temp, hpr, 'row');
        % upsample columns and low filter
        temp = zeros(lrow, lcol); temp(1:2:end, :) = ll + hl;
        l = symconv2(temp, lpr, 'col');

        % upsample rows and high filter
        temp = zeros(sHH(1), lcol); temp(:, 1:2:end) = lh;
        lh = symconv2(temp, lpr, 'row');
        % upsample rows and high filter
        temp = zeros(sHH(1), lcol); temp(:, 2:2:end) = hh;
        hh = symconv2(temp, hpr, 'row');
        % upsample rows and high filter
        temp = zeros(lrow, lcol); temp(2:2:end, :) = lh + hh;
        h = symconv2(temp, hpr, 'col');

        % update x with the new ll, ie. l+h
        x(1:lrow, 1:lcol) = l + h;
    end
    % output
    c = x;
end
%------------------------- internal function  --------------------------%
%       2-dimension convolution with edges symmetrically extended       %
%-----------------------------------------------------------------------%
function y = symconv2(x, h, direction)
% symmetrically extended convolution(see section 6.5.2 in [1]):
%    x[n], E<=n<=F-1, is extended to x~[n] = x[n], 0<=n<=N-1;
%                                  x~[E-i] = x~[E+i], for all integer i
%                                x~[F-1-i] = x~[F-1+i], for all integer i
%    For odd-length h[n], to convolve x[n] and h[n], we just need extend x
%    by (length(h)-1)/2  for both left and right edges.
% The symmetric extension handled here is not the same as in Matlab
%  wavelets toolbox nor in [2]. The last two use the following method:
%    x[n], E<=n<=F-1, is extended to x~[n] = x[n], 0<=n<=N-1;
%                                  x~[E-i] = x~[E+i-1], for all integer i
%                                x~[F-1-i] = x~[F+i], for all integer i

l = length(h); s = size(x);
lext = (l-1)/2; % length of h is odd
h = h(:)'; % make sure h is row vector
y = x;
if strcmp(direction, 'row') % convolving along rows
    if ~isempty(x) && s(2) > 1 % unit length array skip convolution stage
        for i = 1: lext
            x = [x(:, 2*i), x, x(:, s(2)-1)]; % symmetric extension
        end
        x = conv2(x, h);
        y = x(:, l:s(2)+l-1);
    end
elseif strcmp(direction, 'col') % convolving along columns
    if ~isempty(x) && s(1) > 1 % unit length array skip convolution stage
        for i = 1: lext
            x = [x(2*i, :); x; x(s(1)-1, :)]; % symmetric extension
        end
        x = conv2(x, h');
        y = x(l:s(1)+l-1, :);
    end
end
% EOF


function R = SigmoidResponse(img, sr_n, sr_sigma, sr_B)
%
%       R = SigmoidResponse(img, sr_n, sr_sigma, sr_B)
%
%       This function computes sigmoid response
%
%       input:
%           -img: an image
%           -sr_n: power
%           -sr_sigma: saturation parameter
%           -sr_B:
%
%       output:
%           -R: is the response
%
%     Copyright (C) 2011-14  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

if(~exist('sr_n','var'))
    sr_n = 0.73;
end

if(~exist('sr_sigma','var'))
    sr_sigma = 1.0;
end

if(~exist('sr_B','var'))
    sr_B = 1.0;
end

img_n = img.^sr_n;
R = img_n ./ (img_n + sr_sigma.^sr_n);
R = R * sr_B;

end


function sigma = snr2sigma(X,SNR)
%SNR2SIGMA Summary of this function goes here
%   Detailed explanation goes here
[N,B] = size(X);
p = mean(sum(X.^2,2));
sigma2 = p/(B*10^(SNR/10));
sigma = sqrt(sigma2);

end


function I = mi_gd2(x, y, Ym, biascorrect, demeaned, class_means)
% MI_GD Mutual information (MI) between a Gaussian and a discrete
%         variable in bits
%   I = mi_gd(x,y,Ym) returns the MI between the (possibly multidimensional)
%   Gaussian variable x and the discrete variable y.
%   Rows of x correspond to samples, columns to dimensions/variables.
%   (Samples first axis)
%   y should contain integer values in the range [0 Ym-1] (inclusive).
%
%   biascorrect : true / false option (default true) which specifies whether
%   bias correction should be applied to the esimtated MI.
%   demeaned : false / true option (default false) which specifies whether the
%   input data already has zero mean (true if it has been copula-normalized)

persistent previous_nvar previous_y bias_unc bias_cond;

% ensure samples first axis for vectors
if isvector(x)
    x = x(:);
end
if ndims(x)~=2
    error('mi_gd: input arrays should be 2d')
end
if isvector(y)
    y = y(:);
else
%    error('mi_gd: only univariate discrete variable supported');
end

Ntrl = size(x,1);
Nvar = size(x,2);

if isequal(previous_y, y)
  computebias = false;
else
  computebias = true;
end

if ~isequal(previous_nvar, Nvar)
  computebias = true;
end

if size(y,1) ~= Ntrl
    error('mi_gd: number of trials do not match');
end

% default option values
if nargin<4
    biascorrect = true;
end
if nargin<5
    demeaned = false;
end

if ~demeaned
    x = bsxfun(@minus,x,sum(x,1)/Ntrl);
end

% class-conditional entropies
Ntrl_y = sum(y);
Hcond = zeros(1,Ym);
for yi=1:Ym
    xm = x(y(:,yi),:);
    %Ntrl_y(yi) = size(xm,1);
    %xm = bsxfun(@minus,xm,sum(xm,1)/Ntrl_y(yi));
    Cm = (xm'*xm) / (Ntrl_y(yi) - 1);
    chCm = chol(Cm);
    Hcond(yi) = sum(log(diag(chCm)));% + c*Nvar;
end
% class weights
w = Ntrl_y ./ Ntrl;

% input data is class-demeaned, this needs to be accounted for in the
% unconditional entropies
c = diag(sqrt(Ntrl_y))*class_means;%*diag(Ntrl_y);

% unconditional entropy from unconditional Gaussian fit
Cx = (x'*x + c'*c) / (Ntrl-1);
chC = chol(Cx);
Hunc = sum(log(diag(chC)));% + c*Nvar; % the commented out bit drops out in the subtraction below

% apply bias corrections
ln2 = log(2);
if biascorrect

    if computebias,
      vars = 1:Nvar;

      psiterms_unc = psi((Ntrl - vars)/2) / 2;
      dterm_unc    = (ln2 - log(Ntrl-1)) / 2;
      bias_unc     = Nvar*dterm_unc + sum(psiterms_unc);

      dterm_cond    = (ln2 - log(Ntrl_y-1)) / 2;
      psiterms_cond = zeros(1,Ym);
      for vi=vars
        idx = (Ntrl_y-vi);
        psiterms_cond = psiterms_cond + psi(idx/2);
      end
      bias_cond = Nvar*dterm_cond + (psiterms_cond/2);
    end

    Hunc  = Hunc  - bias_unc;
    Hcond = Hcond - bias_cond;
end

I = Hunc - w*Hcond(:);% sum(w .* Hcond);
% convert to bits
I = I / ln2;

previous_y = y;
previous_nvar = Nvar;


function [nScales, scale_step, scaleFactors, scale_filter, params] = init_scale_filter(params)

% Initialize the scale filter parameters. Uses the fDSST scale filter.

init_target_sz = params.init_sz(:)';

nScales = params.number_of_scales_filter;
scale_step = params.scale_step_filter;

scale_sigma = params.number_of_interp_scales * params.scale_sigma_factor;

scale_exp = (-floor((nScales-1)/2):ceil((nScales-1)/2)) * params.number_of_interp_scales/nScales;
scale_exp_shift = circshift(scale_exp, [0 -floor((nScales-1)/2)]);

interp_scale_exp = -floor((params.number_of_interp_scales-1)/2):ceil((params.number_of_interp_scales-1)/2);
interp_scale_exp_shift = circshift(interp_scale_exp, [0 -floor((params.number_of_interp_scales-1)/2)]);

scale_filter.scaleSizeFactors = scale_step .^ scale_exp;
scale_filter.interpScaleFactors = scale_step .^ interp_scale_exp_shift;

ys = exp(-0.5 * (scale_exp_shift.^2) /scale_sigma^2);
scale_filter.yf = single(fft(ys));
scale_filter.window = single(hann(size(ys,2)))';

%make sure the scale model is not to large, to save computation time
if params.scale_model_factor^2 * prod(init_target_sz) > params.scale_model_max_area
    params.scale_model_factor = sqrt(params.scale_model_max_area/prod(init_target_sz));
end

%set the scale model size
params.scale_model_sz = max(floor(init_target_sz * params.scale_model_factor), [8 8]);

scale_filter.max_scale_dim = strcmp(params.s_num_compressed_dim,'MAX');
if scale_filter.max_scale_dim
    params.s_num_compressed_dim = length(scale_filter.scaleSizeFactors);
end

% Scale factor for the translation filter
scaleFactors = 1;


function z = trace_inv( Y )

% TRACE_INV   Trace of the inverse of a PSD matrix.
%     For square matrix X, TRACE_INV(X) is TRACE(INV(X)) if X is Hermitian
%     or symmetric and positive definite; and +Inf otherwise.
%
%     An error results if X is not a square matrix.
%
%     Disciplined convex programming information:
%         TRACE_INV is convex and nonmonotonic (at least with respect to
%         elementwise comparison), so its argument must be affine.

error( nargchk( 1, 1, nargin ) );
if ndims( Y ) > 2 || size( Y, 1 ) ~= size( Y, 2 ),
    error( 'Input must be a square matrix.' );
end
err = Y - Y';
Y   = 0.5 * ( Y + Y' );
if norm( err, 'fro' )  > 8 * eps * norm( Y, 'fro' ),
    z = Inf;
else
    z = eig( full( Y ) );
    if any( z <= 0 ),
        z = Inf;
    else
        z = sum(1.0./z);
    end
end

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function [xo,N]=gabglasso(ttype,xi,lambda,group);
%GABGLASSO   group lasso estimate (hard/soft) in time-frequency domain
%   Usage:  xo=gabglasso(ttype,x,lambda,group);
%           [xo,N]=gabglasso(ttype,x,lambda,group));
%
%   GABGLASSO('hard',x,lambda,'time') will perform
%   time hard group thresholding on x, i.e. all time-frequency
%   columns whose norm less than lambda will be set to zero.
%
%   GABGLASSO('soft',x,lambda,'time') will perform
%   time soft thresholding on x, i.e. all time-frequency
%   columns whose norm less than lambda will be set to zero,
%   and those whose norm exceeds lambda will be multiplied
%   by (1-lambda/norm).
%
%   GABGLASSO(ttype,x,lambda,'frequency') will perform
%   frequency thresholding on x, i.e. all time-frequency
%   rows whose norm less than lambda will be soft or hard thresholded
%   (see above).
%
%   [xo,N]=GABGLASSO(ttype,x,lambda,group) additionally returns
%   a number N specifying how many numbers where kept.
%
%   The function may meaningfully be applied to output from DGT, WMDCT or
%   from WIL2RECT(DWILT(...)).
%
%   See also:  gablasso, gabelasso
%
%   Demos: demo_audioshrink

%   AUTHOR : Bruno Torresani.
%   REFERENCE: OK

complainif_argnonotinrange(nargin,4,4,mfilename);

NbFreqBands = size(xi,1);
NbTimeSteps = size(xi,2);

xo = zeros(size(xi));

switch(lower(group))
 case {'time'}
  for t=1:NbTimeSteps,
    threshold = norm(xi(:,t));
    mask = (1-lambda/threshold);
    if(strcmp(ttype,'soft'))
      mask = mask * (mask>0);
    elseif(strcmp(ttype,'hard'))
      mask = (mask>0);
    end
    xo(:,t) = xi(:,t) * mask;
  end
 case {'frequency'}
  for f=1:NbFreqBands,
    threshold = norm(xi(f,:));
    mask = (1-lambda/threshold);
    mask = mask * (mask>0);
    if(strcmp(ttype,'soft'))
      mask = mask * (mask>0);
    elseif(strcmp(ttype,'hard'))
      mask = (mask>0);
    end
    xo(f,:) = xi(f,:) * mask;
  end
 otherwise
  error('"group" parameter must be either "time" or "frequency".');
end

if nargout==2
    signif_map = (abs(xo)>0);
    N = sum(signif_map(:));
end


%COVM Compute covariance matrix for large datasets
%
% 	C = COVM(A)
%
% Similar to C = COV(A) this routine computes the covariance matrix
% for the datavectors stored in the rows of A. No large intermediate
% matrices are created. If class(A) is 'prdataset' then class(C) is
% 'double'.

% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

% $Id: covm.m,v 1.3 2010/02/08 15:34:14 duin Exp $

function c = covm(a,n)
		[m,k] = size(a);
if nargin < 2, n = 0; end
if n ~= 1 & n ~= 0
	error('Second parameter should be either 0 or 1')
end
[loops,n0,n1] = prmem(m,k);
if loops == 1
	c = prcov(+a,n);
	c = (c+c')/2;
	return
end
c = zeros(k,k);
u = ones(n0,1)*mean(a);
for j = 1:loops
	if j == loops, n = n1; else n = n0; end
	nn = (j-1)*n0;
	b = +a(nn+1:nn+n,:) - u(1:n,:);
	c = c + b'*b;
end
c = (c + c')/2;
if n
	c = c/m;
else
	c = c/(m-1);
end


function g = mult_givens ( c, s, k, g )

%*****************************************************************************80
%
%% MULT_GIVENS applies a Givens rotation to two successive entries of a vector.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 March 2008
%
%  Author:
%
%    C original version by Lili Ju
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Richard Barrett, Michael Berry, Tony Chan, James Demmel,
%    June Donato, Jack Dongarra, Victor Eijkhout, Roidan Pozo,
%    Charles Romine, Henk van der Vorst,
%    Templates for the Solution of Linear Systems:
%    Building Blocks for Iterative Methods,
%    SIAM, 1994.
%    ISBN: 0898714710,
%    LC: QA297.8.T45.
%
%    Tim Kelley,
%    Iterative Methods for Linear and Nonlinear Equations,
%    SIAM, 2004,
%    ISBN: 0898713528,
%    LC: QA297.8.K45.
%
%    Yousef Saad,
%    Iterative Methods for Sparse Linear Systems,
%    Second Edition,
%    SIAM, 2003,
%    ISBN: 0898715342,
%    LC: QA188.S17.
%
%  Parameters:
%
%    Input, real C, S, the cosine and sine of a Givens
%    rotation.
%
%    Input, integer K, indicates the location of the first vector entry.
%
%    Input/output, real G(1:K+1), the vector to be modified.  On output,
%    the Givens rotation has been applied to entries G(K) and G(K+1).
%
  g1 = c * g(k) - s * g(k+1);
  g2 = s * g(k) + c * g(k+1);

  g(k)   = g1;
  g(k+1) = g2;

  return
end


function M = euclideanfactory(m, n)
% Returns a manifold struct to optimize over real matrices.
%
% function M = euclideanfactory(m)
% function M = euclideanfactory(m, n)
% function M = euclideanfactory([n1, n2, ...])
%
% Returns M, a structure describing the Euclidean space of real matrices,
% equipped with the standard Frobenius distance and associated trace inner
% product, as a manifold for Manopt.
%
% m and n in general can be vectors to handle multidimensional arrays.
% If either of m or n is a vector, they are concatenated as [m, n].
%
% Using this simple linear manifold, Manopt can be used to solve standard
% unconstrained optimization problems, for example in replacement of
% Matlab's fminunc.
%
% See also: euclideancomplexfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors: Bamdev Mishra, May 4, 2015.
% Change log:
%
%   July 5, 2013 (NB):
%       Added egred2rgrad, ehess2rhess, mat, vec, tangent.
%   May 4, 2015 (BM):
%       Added functionality to handle multidimensional arrays.

    % The size can be defined using both m and n, or simply with m.
    % If m is a scalar, then n is implicitly 1.
    % This mimics the use of built-in Matlab functions such as zeros(...).
    if ~exist('n', 'var') || isempty(n)
        if numel(m) == 1
            n = 1;
        else
            n = [];
        end
    end

    dimensions_vec = [m(:)', n(:)']; % We have a row vector.

    M.size = @() dimensions_vec;

    M.name = @() sprintf('Euclidean space R^(%s)', num2str(dimensions_vec));

    M.dim = @() prod(dimensions_vec);

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

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

    M.dist = @(x, y) norm(x(:) - y(:), 'fro');

    M.typicaldist = @() sqrt(prod(dimensions_vec));

    M.proj = @(x, d) d;

    M.egrad2rgrad = @(x, g) g;

    M.ehess2rhess = @(x, eg, eh, d) eh;

    M.tangent = M.proj;

    M.exp = @exp;
    function y = exp(x, d, t)
        if nargin == 3
            y = x + t*d;
        else
            y = x + d;
        end
    end

    M.retr = M.exp;

    M.log = @(x, y) y-x;

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

    M.rand = @() randn(dimensions_vec);

    M.randvec = @randvec;
    function u = randvec(x) %#ok<INUSD>
        u = randn(dimensions_vec);
        u = u / norm(u(:), 'fro');
    end

    M.lincomb = @matrixlincomb;

    M.zerovec = @(x) zeros(dimensions_vec);

    M.transp = @(x1, x2, d) d;
    M.isotransp = M.transp; % the transport is isometric

    M.pairmean = @(x1, x2) .5*(x1+x2);

    M.vec = @(x, u_mat) u_mat(:);
    M.mat = @(x, u_vec) reshape(u_vec, dimensions_vec);
    M.vecmatareisometries = @() true;
    M.lie_identity = @() zeros(dimensions_vec);

end


function r8gb_print ( m, n, ml, mu, a, title )

%*****************************************************************************80
%
%% R8GB_PRINT prints a banded matrix.
%
%  Discussion:
%
%    An M by N banded matrix A with lower bandwidth ML and upper bandwidth MU
%    is assumed to be entirely zero, except for the main diagonal, and
%    entries in the ML nearest subdiagonals, and MU nearest superdiagonals.
%
%    LINPACK and LAPACK "R8GB" storage for such a matrix generally includes
%    room for ML extra superdiagonals, which may be required to store
%    nonzero entries generated during Gaussian elimination.
%
%    The original M by N matrix is "collapsed" downward, so that diagonals
%    become rows of the storage array, while columns are preserved.  The
%    collapsed array is logically 2*ML+MU+1 by N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 April 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows of the matrix.
%    M must be positive.
%
%    Input, integer N, the number of columns of the matrix.
%    N must be positive.
%
%    Input, integer ML, MU, the lower and upper bandwidths.
%    ML and MU must be nonnegative, and no greater than min(M,N)-1..
%
%    Input, real A(2*ML+MU+1,N), the M by N band matrix, stored in LINPACK
%    or LAPACK general band storage mode.
%
%    Input, string TITLE, a title to be printed.
%
  r8gb_print_some ( m, n, ml, mu, a, 1, 1, m, n, title );

  return
end


function value = t_triple_product_integral ( i, j, k )

%*****************************************************************************80
%
%% T_TRIPLE_PRODUCT_INTEGRAL: integral (-1<=x<=1) T(i,x)*T(j,x)*T(k,x)/sqrt(1-x^2) dx
%
%  Discussion:
%
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 April 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    John Mason, David Handscomb,
%    Chebyshev Polynomials,
%    CRC Press, 2002,
%    ISBN: 0-8493-035509,
%    LC: QA404.5.M37.
%
%  Parameters:
%
%    Input, integer I, J, K, the polynomial indices.
%    0 <= I, J.
%
%    Output, real VALUE, the integral.
%
  if ( i < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'T_TRIPLE_PRODUCT_INTEGRAL - Fatal error!\n' );
    fprintf ( 1, '  0 <= I is required.\n' );
    error ( 'T_TRIPLE_PRODUCT_INTEGRAL - Fatal error!' );
  end

  if ( j < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'T_TRIPLE_PRODUCT_INTEGRAL - Fatal error!\n' );
    fprintf ( 1, '  0 <= J is required.\n' );
    error ( 'T_TRIPLE_PRODUCT_INTEGRAL - Fatal error!' );
  end

  if ( k < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'T_TRIPLE_PRODUCT_INTEGRAL - Fatal error!\n' );
    fprintf ( 1, '  0 <= K is required.\n' );
    error ( 'T_TRIPLE_PRODUCT_INTEGRAL - Fatal error!' );
  end

  value = 0.5 * ( ...
      t_double_product_integral (       i + j,   k ) + ...
    + t_double_product_integral ( abs ( i - j ), k ) );

  return
end


%NBAYESC Bayes Classifier for given normal densities
%
%   W = NBAYESC(U,G)
%
% INPUT
%   U  Dataset of means of classes
%   G  Covariance matrices (optional; default: identity matrices)
%
% OUTPUT
% 	W  Bayes classifier
%
% DESCRIPTION
% Computation of the Bayes normal classifier between a set of classes.
% The means, labels and priors are defined by the dataset U of the size
% [C x K]. The covariance matrices are stored in a matrix G of the
% size [K x K x C], where K and C correspond to the dimensionality and
% the number of classes, respectively.
%
% If C is 1, then G is treated as the common covariance matrix, yielding
% a linear solution. For G = I, the nearest mean solution is obtained.
%
% This routine gives the exact solution for the given parameters, while
% the trainable classifiers QDC and LDC give approximate solutions, based
% on the parameter estimates from a training set. For a given dataset, U
% and G can be computed by MEANCOV.
%
% EXAMPLES
% [U,G] = MEANCOV(GENDATB(25));
% W = NBAYESC(U,G);
%
% SEE ALSO (PRTools Guide)
% MAPPINGS, DATASETS, QDC, LDC, NMC.

% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Sciences, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

% $Id: nbayesc.m,v 1.4 2009/01/04 21:11:07 duin Exp $

function W = nbayesc(U,G);

		[cu,ku] = size(U);		% CU is the number of classes and KU - the dimensionality
	if nargin == 1,
		prwarning(4,'Covariance matrix is not specified, the identity matrix is assumed.');
		G = eye(ku);
	end

	[k1,k2,c] = size(G);	% C = 1, if G is the common covariance matrix.

	if (c ~= 1 & c ~= cu) | (k1 ~= k2) | (k1 ~= ku)
		error('Covariance matrix or a set of means has a wrong size.')
	end

	pars.mean  = +U;
	pars.cov   = G;
	pars.prior = getprior(U);

	%W = prmapping('normal_map','trained',pars,getlablist(U),ku,cu);
	W = normal_map(pars,getlablist(U),ku,cu);
	W = setname(W,'BayesNormal');
	W = setcost(W,U);

return;


function [y,p_avg,p_std]=multinomrnd(p,m,n)
%Performs random sampling from a binomial distribution
%
% [y]=multinomrnd(p,m,n)
% where p=1-by-k vector of probabilities of occurrence
%       n=sample size
% and   m= number of trials
%       y=samples-matrix of size k-by-m
%
% for picking out one of k mixture components, set n=1;
%
if nargin<3
  n=1;
end

k=length(p);
x=rand(n,m);

if (sum(p)-1>100*eps)
  p(k+1)=1-sum(p);
  k=k+1;
end
p=cumsum(p);

y(1,:)=sum(x<=p(1),1);
for i=2:k
  y(i,:)=sum(x>p(i-1) & x<=p(i),1);
end

p_avg=mean(y'./n);
p_std=std(y'./n);


%% Housekeeping
clc
clear
close all

%% Using the Grobner solver: constant-parameter case
m0=rise('rbcs0');
m1=solve(m0);
print_solution(m1)

% Finding all solutions
%-----------------------
m2=solve(m0,'solver','dsge_groebner','solve_check_stability',false);
print_solution(m2)
print_solution(m1)

%% Using the Grobner solver: regime switching example
clc

m00=rise('rbcs','solve_perturbation_type',{'frwz',{'mu'}});

% Finding one solution
%----------------------
m01=solve(m00);

% Finding all solutions
%-----------------------
m02=solve(m00,'solver','dsge_groebner','solve_check_stability',false);
print_solution(m01)
print_solution(m02)

%% Using the dsge_udc solver: regime switching example
clc

m000=set(m00,'solve_perturbation_type','mw');

% Finding one solution
%----------------------
m001=solve(m000);

% Finding all solutions Groebner
%-------------------------------
m002=solve(m000,'solver','dsge_groebner','solve_check_stability',false);

% Finding all solutions UDC
%--------------------------
refine=false;checkStab=false;allSols=true;msvOnly=true;
xplosRoots=false;debug=false;
solver={'dsge_udc',refine,checkStab,allSols,msvOnly,xplosRoots,debug};
m003=solve(m000,'solver',solver,'solve_check_stability',false);

% Finding all solutions UDC with refinement
%------------------------------------------
refine=true;checkStab=false;allSols=true;msvOnly=true;
xplosRoots=false;debug=false;
solver={'dsge_udc',refine,checkStab,allSols,msvOnly,xplosRoots,debug};

m004=solve(m000,'solver',solver,'solve_check_stability',false);


function d = fd03 ( p )

%*****************************************************************************80
%
%% FD03 is a signed distance function for problem 3.
%
%  Discussion:
%
%    The formula used here is not quite correct.  In particular, it is wrong
%    for points exterior to the cube whose nearest point on the cube is at a corner.
%
%    For DISTMESH_3D's purposes, though, this computation is accurate enough.
%
%  Modified:
%
%    12 September 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real P(N,3), one or more points.
%
%    Output, real D(N), the signed distance of each point to the boundary of the region.
%
  d = - min ( min ( min ( min ( min ( -0.0+p(:,3), 1.0-p(:,3) ), ...
                                     -0.0+p(:,2) ), ...
                                      1.0-p(:,2) ), ...
                                     -0.0+p(:,1) ), ...
                                      1.0-p(:,1) );

  return
end


classdef SymbolicVoigtRotationMatrixGenerator < handle

    properties (Access = public)
        VoigtRotationMatrix
    end

    properties (Access = private)
        RotationMatrix
        Stress
        RotatedStress
    end

    methods (Access = public)

        function obj = SymbolicVoigtRotationMatrixGenerator()
            obj.createRotationMatrix();
            obj.createStressTensor();
            obj.computeRotatedStressTensor();
            obj.obtainVoigtRotationMatrix()
        end

    end

    methods (Access = private)

        function obj = createRotationMatrix(obj)
            theta    = obj.createRotationAngle();
            vect     = obj.createNormalVector();
            rotator = VectorRotator(theta,vect);
            obj.RotationMatrix = rotator.getRotationMatrix();
        end

        function u = createNormalVector(obj)
            u = sym('u',[3 1],'real');
        end

        function theta = createRotationAngle(obj)
            theta = sym('theta','real');
        end

        function createStressTensor(obj)
            obj.Stress = StressTensor();
            obj.Stress.tensor = sym('s',[3 3],'real');
            Tens = obj.Stress.tensor;
            Tens = obj.Stress.symmetrizeWithUpperDiagonal(Tens);
            obj.Stress.tensor = Tens;
            obj.Stress.transformTensor2Voigt();
        end

        function computeRotatedStressTensor(obj)
            R = obj.RotationMatrix;
            S = obj.Stress.tensor;
            RotS = R'*S*R;
            obj.RotatedStress = StressTensor();
            obj.RotatedStress.tensor = simplify(RotS);
            obj.RotatedStress.transformTensor2Voigt();
        end

        function obtainVoigtRotationMatrix(obj)
            RotStre = obj.RotatedStress.tensorVoigt;
            Stre    = obj.Stress.tensorVoigt;
            DimVoigt = length(RotStre);
            RotMatrix = sym(zeros(DimVoigt,DimVoigt));
            for iStress = 1:DimVoigt
                RS = RotStre(iStress);
                for jStress = 1:DimVoigt
                    S = Stre(jStress);
                    [TensorValue,~] = coeffs(RS,S);
                    TensorValue = obj.takeApropiateComponent(TensorValue);
                    RotMatrix(iStress,jStress) = TensorValue;
                end
            end
            obj.VoigtRotationMatrix = simplify(RotMatrix);
        end

        function value = takeApropiateComponent(obj,value)
            if isempty(value)
                value = [];
            else
                Dim = length(value);
                if Dim == 1
                    value = 0;
                else
                    value = value(1);
                end
            end
        end

    end

end


function v = integral2(F, S)
%INTEGRAL2   Flux integral of a Chebfun3v object through a 2D-surface.
%   INTEGRAL2(F, S) computes the flux integral of the Chebfun3v object F
%   through the parametric surface S defined as a Chebfun2v object (with 3
%   components):
%                           /
%       INTEGRAL2(F, S) =  |   < F, dS >
%                          /S
%                  //
%               = ||   < F(S(x,y)), cross(S_x(x,y), S_y(x,y)) > dxdy.
%                 //D
%
% See also CHEBFUN3V/INTEGRAL, CHEBFUN3/INTEGRAL, CHEBFUN3/INTEGRAL2,
% CHEBFUN3/INTEGRAL3, CHEBFUN3/SUM, CHEBFUN3/SUM2 and CHEBFUN3/SUM3.

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

% Check that F is a chebfun3v object with 3 components:
if ( F.nComponents ~= 3 )
    error('CHEBFUN:CHEBFUN3V:integral2:dimChebfun3v', ...
        'The chebfun3v object must have 3 components.')
end

% Check that S is a chebfun2v object with 3 components:
if ( isa(S, 'chebfun2v') )
    if ( S.nComponents ~= 3 )
        error('CHEBFUN:CHEBFUN3V:integral2:dimChebfun2v', ...
            'The parametrisation must have 3 components.')
    end
else
    error('CHEBFUN:CHEBFUN3V:integral2:notaChebfun2v', ...
        'The parametrisation must be a chebfun2v object.')
end

% Get components of F:
F1 = F.components{1};
F2 = F.components{2};
F3 = F.components{3};

% Get components of the surface S:
S1 = S(1);
S2 = S(2);
S3 = S(3);

% Build the composition F(S):
FS1_handle = @(x,y) feval(F1, feval(S1, x, y), feval(S2, x, y), ...
    feval(S3, x, y));

FS2_handle = @(x,y) feval(F2, feval(S1, x, y), feval(S2, x, y), ...
    feval(S3, x, y));

FS3_handle = @(x,y) feval(F3, feval(S1, x, y), feval(S2, x, y), ...
    feval(S3, x, y));

FS = chebfun2v(FS1_handle, FS2_handle, FS3_handle, S1.domain);

% Normal vector to the surface:
dS = normal(S);

% By definition:
v = sum2(dot(FS, dS));

end


function y = logWishart(Sigma, W, v)
% Compute log pdf of a Wishart distribution.
% Input:
%   Sigma: d x d covariance matrix
%   W: d x d covariance parameter
%   v: degree of freedom
% Output:
%   y: probability density in logrithm scale y=log p(Sigma)
% Written by Mo Chen (sth4nth@gmail.com).
d = length(Sigma);
B = -0.5*v*logdet(W)-0.5*v*d*log(2)-logmvgamma(0.5*v,d);
y = B+0.5*(v-d-1)*logdet(Sigma)-0.5*trace(W\Sigma);


function dUpsilonS = lfmvvGradientSigmaUpsilonMatrix(gamma, sigma2, ...
    t1, t2, mode)

% LFMVVGRADIENTSIGMAUPSILONMATRIX Gradient of upsilon matrix vv wrt sigma
% FORMAT
% DESC computes the gradient wrt sigma of a portion of the LFMVV kernel.
% ARG gamma : Gamma value for system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG mode : operation mode, according to the derivative (mode 0,
% derivative wrt t1, mode 1 derivative wrt t2)
% RETURN upsilon : result of this subcomponent of the kernel for the given values.
%
% COPYRIGHT : Mauricio Alvarez, 2010
%
% SEEALSO : lfmvvComputeUpsilonMatrix.m

% KERN

gridt1 = repmat(t1, 1, length(t2));
gridt2 = repmat(t2', length(t1), 1);
timeGrid = gridt1 - gridt2;

dUpsilon = lfmvpGradientSigmaUpsilonMatrix(gamma, sigma2, t1, t2, mode);

if mode == 0
    dUpsilonS = gamma*dUpsilon - 4*timeGrid/(sqrt(pi)*sigma2^2).* ...
        exp(-(timeGrid.^2)./sigma2).*(3 - 2*(timeGrid.^2)/sigma2) ...
        + 2*gamma/(sqrt(pi)*sigma2)*exp(-gamma*t1)*((1-2*t2.^2/sigma2).* ...
        exp(-(t2.^2)/sigma2)).';
else
    dUpsilonS = -gamma*dUpsilon - 4*timeGrid/(sqrt(pi)*sigma2^2).* ...
        exp(-(timeGrid.^2)./sigma2).*(3 - 2*(timeGrid.^2)/sigma2);
end


function Cbn = att2Cbn(att);
%--------------------------------------------------------------------------
% calculate the direct cosine matrix c_nb from b => n-frame given att (h,p,r)
% assume 1 row or col input
% c_nb = r3(-h)*r2(-p)*r1(-r);
% Author:   Yudan Yi
%           Aug. 2004
%           Feb. 2005
%--------------------------------------------------------------------------
if (nargin<1) error('error in data'); end;
att = att(:);
if (length(att)<3) error('error in data'); end;
%H = att(1); P = att(2); R = att(3);
R = att(1); P = att(2); H = att(3);
Cbn(1,1)	= cos(H)*cos(P);
Cbn(2,1)	= sin(H)*cos(P);
Cbn(3,1)	=-	     sin(P);
Cbn(1,2)	=-sin(H)*cos(R)+cos(H)*sin(P)*sin(R);
Cbn(2,2)	= cos(H)*cos(R)+sin(H)*sin(P)*sin(R);
Cbn(3,2)	=                      cos(P)*sin(R);
Cbn(1,3)	= sin(H)*sin(R)+cos(H)*sin(P)*cos(R);
Cbn(2,3)	=-cos(H)*sin(R)+sin(H)*sin(P)*cos(R);
Cbn(3,3)	=                      cos(P)*cos(R);
%--------------------------------------------------------------------------


function cheby_u_poly_coef_test ( )

%*****************************************************************************80
%
%% CHEBY_U_POLY_COEF_TEST tests CHEBY_U_POLY_COEF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2008
%
%  Author:
%
%    John Burkardt
%
  n = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CHEBY_U_POLY_COEF_TEST\n' );
  fprintf ( 1, '  CHEBY_U_POLY_COEF determines the Chebyshev U \n' );
  fprintf ( 1, '  polynomial coefficients.\n' );

  c = cheby_u_poly_coef ( n );

  for i = 0 : n
    fprintf ( 1, '\n' );
    fprintf ( 1, '  U(%d)\n', i );
    fprintf ( 1, '\n' );
    for j = i : -1 : 0
      if ( j == 0 )
        fprintf ( 1, '    %f\n', c(i+1,j+1) );
      elseif ( j == 1 )
        fprintf ( 1, '    %f * x\n', c(i+1,j+1) );
      else
        fprintf ( 1, '    %f * x^%d\n', c(i+1,j+1), j );
      end
    end
  end

  return
end


function [e1,e2,E1_l,E2_l,E1_s1,E2_s2] = pluckerEndpoints(L,s1,s2)

% PLUCKERENDPOINTS  Plucker line and abscissas to endpoints conversion.
%   [E1,E2] = PLUCKERENDPOINTS(L,S1,S2) are the endpoints of the Plucker
%   line L at abscissas S1 and S2.
%
%   [E1,E2,E1_l,E2_l,E1_s1,E2_s2] = PLUCKERENDPOINTS(L,S1,S2) returns the
%   Jacobians wrt the line L and the abscissas S1 and S2.
%
%   See also LS2E, LS2SEG.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

v = L(4:6);

if nargout == 1

    vn = normvec(v);
    p0 = pluckerOrigin(L);
    e1 = p0 + s1*vn;
    e2 = p0 + s2*vn;

else % Jac

    [vn,VN_v] = normvec(v,0);
    [p0,P0_l] = pluckerOrigin(L);

    P0_n  = P0_l(:,1:3);
    P0_v  = P0_l(:,4:6);

    e1    = p0 + s1*vn;
    E1_s1 = vn;

    E1_n  = P0_n;
    E1_v  = P0_v + s1*VN_v;

    E1_l  = [E1_n E1_v];

    e2    = p0 + s2*vn;
    E2_s2 = vn;

    E2_n  = P0_n;
    E2_v  = P0_v + s2*VN_v;

    E2_l  = [E2_n E2_v];

end

return

%% jac

syms n1 n2 n3 v1 v2 v3 s1 s2 real
L = [n1;n2;n3;v1;v2;v3];

[e1,e2,E1_l,E2_l,E1_s1,E2_s2] = pluckerEndpoints(L,s1,s2);

simplify(E1_l - jacobian(e1,L))
simplify(E2_l - jacobian(e2,L))
simplify(E1_s1 - jacobian(e1,s1))
simplify(E2_s2 - jacobian(e2,s2))

% ========== 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 value = index0n ( n, i_min, i, i_max )

%*****************************************************************************80
%
%% INDEX0N indexes an N-dimensional array by columns, with zero base.
%
%  Discussion:
%
%    Entries of the array are indexed starting at entry
%      ( I_MIN(1), I_MIN(2),...,I_MIN(N) ),
%    and increasing the first index up to I_MAX(1),
%    then the second and so on.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 November 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of indices.
%
%    Input, integer I_MIN(N), the minimum indices.
%
%    Input, integer I(N), the indices.
%
%    Input, integer I_MAX(N), for maximum indices.
%
%    Output, integer VALUE, the index of element I.
%
  index_min = 0;

  value = ( i(n) - i_min(n) );

  for j = n - 1 : -1 : 1
    value = value * ( i_max(j) + 1 - i_min(j) ) + ( i(j) - i_min(j) );
  end
  value = value + index_min;

  return
end


function ray = createRay(varargin)
%CREATERAY Create a ray (half-line), from various inputs.
%
%   RAY = createRay(POINT, ANGLE)
%   POINT is a N*2 array giving starting point of the ray, and ANGLE is the
%   orientation of the ray.
%
%   RAY = createRay(X0, Y0, ANGLE)
%   Specify ray origin with 2 input arguments.
%
%   RAY = createRay(P1, P2)
%   Create a ray starting from point P1 and going in the direction of point
%   P2.
%
%   Ray is represented in a parametric form: [x0 y0 dx dy]
%   x = x0 + t*dx
%   y = y0 + t*dy;
%   for all t>0
%
%   Example
%   origin  = [3 4];
%   theta   = pi/6;
%   ray = createRay(origin, theta);
%   figure(1); clf; hold on;
%   axis([0 10 0 10]);
%   drawRay(ray);
%
%   See also
%   rays2d, createLine, points2d

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2007-10-18
% Copyright 2007-2022 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas

if length(varargin)==2
    p0 = varargin{1};
    arg = varargin{2};
    if size(arg, 2)==1
        % second input is the ray angle
        ray = [p0 cos(arg) sin(arg)];
    else
        % second input is another point
        ray = [p0 arg-p0];
    end

elseif length(varargin)==3
    x = varargin{1};
    y = varargin{2};
    theta = varargin{3};
    ray = [x y cos(theta) sin(theta)];

else
    error('Wrong number of arguments in ''createRay'' ');
end


function PDP=ieee802_11_model(sigma_tau,Ts)
% IEEE 802.11 channel model PDP generator
% Input:
%       sigma_tau  : RMS delay spread
%       Ts         : Sampling time
% Output:
%       PDP        : Power delay profile

lmax = ceil(10*sigma_tau/Ts);
sigma02=(1-exp(-Ts/sigma_tau))/(1-exp(-(lmax+1)*Ts/sigma_tau)); % (2.9)
l=0:lmax;
PDP = sigma02*exp(-l*Ts/sigma_tau); % (2.8)


function oeprint1(mu, oev)

% print six classical orbital elements
% and orbital period in minutes

% input

%  mu     = gravitational constant (km**3/sec**2)
%  oev(1) = semimajor axis (kilometers)
%  oev(2) = orbital eccentricity (non-dimensional)
%           (0 <= eccentricity < 1)
%  oev(3) = orbital inclination (radians)
%           (0 <= inclination <= pi)
%  oev(4) = argument of perigee (radians)
%           (0 <= argument of perigee <= 2 pi)
%  oev(5) = right ascension of ascending node (radians)
%           (0 <= raan <= 2 pi)
%  oev(6) = true anomaly (radians)
%           (0 <= true anomaly <= 2 pi)

% Orbital Mechanics with MATLAB

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

rtd = 180 / pi;

% unload orbital elements array

sma = oev(1);
ecc = oev(2);
inc = oev(3);
argper = oev(4);
raan = oev(5);
tanom = oev(6);

arglat = mod(tanom + argper, 2.0 * pi);

if (sma > 0.0)
    period = 2.0d0 * pi * sma * sqrt(sma / mu);
else
    period = 99999.9;
end

% print orbital elements

fprintf ('\n        sma (km)              eccentricity          inclination (deg)         argper (deg)');

fprintf ('\n %+16.14e  %+16.14e  %+16.14e  %+16.14e \n', sma, ecc, inc * rtd, argper * rtd);

fprintf ('\n       raan (deg)          true anomaly (deg)         arglat (deg)            period (min)');

fprintf ('\n %+16.14e  %+16.14e  %+16.14e  %+16.14e \n', raan * rtd, tanom * rtd, arglat * rtd, period / 60);


function [AB]=vectorTensorProductArray(a,b)

%%

%Determine multiplication order
if size(a,2)==3 %type B*A
   B=a;
   A=b;
   multType='post';
else %type A*B
    B=b;
    A=a;
    multType='pre';
end

%Perform product
switch multType
    case 'pre' %type A*B
        %  A1_1*B1 + A1_2*B2 + A1_3*B3
        %  A2_1*B1 + A2_2*B2 + A2_3*B3
        %  A3_1*B1 + A3_2*B2 + A3_3*B3
        AB=[A(:,1).*B(:,1)+A(:,4).*B(:,2)+A(:,7).*B(:,3) ...
            A(:,2).*B(:,1)+A(:,5).*B(:,2)+A(:,8).*B(:,3) ...
            A(:,3).*B(:,1)+A(:,6).*B(:,2)+A(:,9).*B(:,3)];
    case 'post' %type B*A
        %  A1_1*B1 + A1_2*B2 + A1_3*B3
        %  A2_1*B1 + A2_2*B2 + A2_3*B3
        %  A3_1*B1 + A3_2*B2 + A3_3*B3
        AB=[A(:,1).*B(:,1)+A(:,2).*B(:,2)+A(:,3).*B(:,3) ...
            A(:,4).*B(:,1)+A(:,5).*B(:,2)+A(:,6).*B(:,3) ...
            A(:,7).*B(:,1)+A(:,8).*B(:,2)+A(:,9).*B(:,3)];
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function v = norm(F)
%NORM   Frobenius norm of a CHEBFUN3V object.
%   V = NORM(F) returns the Frobenius norm of F, i.e.
%       V = sqrt(norm(F1).^2 + norm(F2).^2),
%   or
%       V = sqrt(norm(F1).^2 + norm(F2).^2 + norm(F3).^2) .
%   where F = [F1 F2 F3]^T.

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

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

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

end


% By Roger Aarenstrup, roger.aarenstrup@mathworks.com
% 2006-08-18
%
% This is a state-space DC motor model of a
% Maxon RE25 10 Watt, precious metal brushes, 118743
%
% This model also have a weak connection to a load.
%

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%            The full dc motor model                      %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Vin is the input voltage to the motor
% i is the motor current
% th_m is the rotor angle, theta
% dth_m is the rotor angular velocity sometimes called omega
% th_l is the load angle
% dth_l is the load angular velocity

% Controller Sample Time
Ts = 100e-6;

% PARAMETERS DC MOTOR
Rm  = 2.06;             % Motor resistance (ohm)
Lm  = 0.000238;         % motor inductance (Henrys)
Kb = 1/((406*2*pi)/60); % Back EMF constant (Volt-sec/Rad)
Kt = 0.0235;            % Torque constand (Nm/A)
Jm = 1.07e-6;           % Rotor inertia (Kg m^2)
bm = 12e-7;             % MEchanical damping (linear model of
                        % friction: bm * dth)
% PARAMETERS LOAD
Jl = 10.07e-6;   % Load inertia (10 times the rotor)
bl = 12e-6;      % Load damping (friction)
Ks = 100;        % Spring constant for connection rotor/load
b = 0.0001;      % Spring damping for connection rotor/load

% SYSTEM MATRICES
%
% States:  [i dth_m th_m dth_l th_l]'
% Input:   Vin the motor voltage
% Outputs: same as states
%
Afull = [-Rm/Lm    -Kb/Lm       0       0       0;
         Kt/Jm   -(bm+b)/Jm   -Ks/Jm    b/Jm     Ks/Jm;
           0          1         0       0        0;
           0         b/Jl    Ks/Jl   -(b+bl)/Jl    -Ks/Jl;
           0          0         0       1        0];

Bfull = [1/Lm 0 0 0 0]';

Cfull = [0 1 0 0 0;
         0 0 1 0 0;
         0 0 0 1 0;
         0 0 0 0 1];

Dfull = [0 0 0 0]';

sys_full = ss(Afull, Bfull, Cfull, Dfull);

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%     The reduced dc motor model for current control      %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% SYSTEM MATRICES
%
% States:  [dth_m th_m dth_l th_l]'
% Input:   I The current to the dc motor
% Outputs: same as states
%
Ared = [ -(bm+b)/Jm   -Ks/Jm    b/Jm     Ks/Jm;
              1         0       0        0;
             b/Jl    Ks/Jl   -(b+bl)/Jl    -Ks/Jl;
              0         0       1        0];

Bred = [Kt/Jm 0 0 0]';

Cred = eye(4);

Dred = [0 0 0 0]';

sys_red = ss(Ared, Bred, Cred, Dred);

% Discrete version of the model (as seen from the controller)
sys_red_d = c2d(sys_red, Ts, 'zoh');

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%         Scaling of reduced state-space model            %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% u = Nu * un
% x = Nx * xn
% y = Ny * yn
% u, x, y are real inputs, states and outputs
% un, xn, yn are normalized

max_current = 0.01; % 10 mA as input
max_pos = 4.1;
max_v = 4.1;

Nu = max_current;
Nx = [max_v 0 0 0; 0 max_pos 0 0; 0 0 max_v 0; 0 0 0 max_pos];
Ny = Nx;

An = inv(Nx)*Ared*Nx;
Bn = inv(Nx)*Bred*Nu;
Cn = inv(Ny)*Cred*Nx;
Dn = 0;

sys_red_n = ss(An, Bn, Cn, Dn);

sys_red_d_n = c2d(sys_red_n, Ts, 'zoh');

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%        State-space controller design attempt 1          %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% The discrete poles of the closed loop system
dpole1 = exp(-2*pi*Ts*[20 22 24 26]);

% Calculate the control parameters
L1 = place(sys_red_d_n.a, sys_red_d_n.b, dpole1);

% Keep the static gain of the closed loop system to 1
F=feedback(sys_red_d_n, L1);   % The closed loop system, F.
Kstatic = freqresp(F, 0);    % Get the static gain of F.
Kstat = 1/Kstatic(4);        % It is the fourth output (load position)

% The above commands requires toolboxes, incase you don't have them
% you can simulate the system with a unit step is see the output static
% gain. Kstat will be the inverse of that.

% to verify the pole placement
%pzmap(F);

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  State-space controller design attempt 2 - Integrator   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% The discrete poles of the closed loop system
dpole2 = exp(-2*pi*Ts*[20 22 24 26 5]);

% We only consider one output here
Cred_one = [0 1 0 0];

% Calculate the control parameters
Aint = [sys_red_d_n.a [0 0 0 0]';
        -Cred_one 1];

Bint= [sys_red_d_n.b' 0]';

Cint = [Cred_one 0];

Dint = [0]';

sys_int_d2 = ss(Aint, Bint, Cint, Dint, Ts);

% Controllability VS Observability analysis
%ob = obsv(sys_int_d2.a, sys_int_d2.c);
%cr = ctrb(sys_int_d2.a, sys_int_d2.b);
%rank(ob)
%rank(cr)

[L2, a, m] = place(sys_int_d2.a, sys_int_d2.b, dpole2);

% Feed Forward gain
Kff = L2(5)/(dpole2(5) - 1);

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  State-space controller design attempt 3 - Observer     %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% These should be about twice as fast as the state feedback
% for the controller.
dpole3 = exp(-2*pi*Ts*[1400 1450 1500 1550]);
%dpole3 = [-0.3 -0.33 -0.35 -0.37];

% Recalculate the reduced model with one ouput

Ared2 = inv(Nx)*Ared*Nx; % Scaling
Bred2 = inv(Nx)*Bred*Nu; % Scaling
Cred2 = [0 1 0 0];
Dred2 = 0;

sys_red2 = ss(Ared2, Bred2, Cred2, Dred2);

% Discrete version of the model (as seen from the controller)
sys_red_d2 = c2d(sys_red2, Ts, 'zoh');

set(sys_red_d2, 'inputname', {'Um'}, ...
                'outputname', {'Enc_out'});

% Calculate the observer state feedback
K = place(sys_red_d2.a', sys_red_d2.c', dpole3);

% Put the observer together
Aobs = [sys_red_d2.a-K'*sys_red_d2.c];
Bobs = [sys_red_d2.b K'];
Cobs = eye(4);
Dobs = [0 0 0 0; 0 0 0 0]';

observer = ss(Aobs, Bobs, Cobs, Dobs, Ts, 'inputname', {'Ue', 'Enc_in'}, ...
              'outputname', {'dth_m', 'th_m', 'dth_l', 'th_l'});

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  State-space controller design attempt 4 - The Servo Case  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Kinv = (Jl+Jm)/Kt;

dpole4 = exp(-2*pi*Ts*[200 204 220 224 300]);
[L3, a, m] = place(sys_int_d2.a, sys_int_d2.b, dpole4);


%% This function will calculate the prediction error between the discovered system and actual test data
% There are two methods to calculate the prediction error.
%
% Method one: This method suits for the one dimensional system only. The idea is
% using ODE45 to drive each point k-steps forward in time and calculate the
% prediction error between the system estimation and actual states. Here,
% the system's estimate is the value of derivative at k-steps.
%
% Method two: This method suits for one dimensional system and
% multi-dimensional system. We use immediate prediction of the system's
% derivative and calculate the prediction error of the system model's
% derivative output and actual derivative.
%
% Last Updated: 2019/06/04
% Coded By: K
%%
function [Score]=Get_Score(dData_test,Data_test,u,Control,tspan,Shuffle,name,Prediction_Steps,dt,size1,size2,method)
% Get the ODE simulation result
Noise=0;

% Start calculate
if method ==1
    % If the method one is selected, then we use the multi step prediction

    % Generate the function handel
    if u==0
        ODE_func=str2func(strcat('@(t,z)',name,'(t,z)'));
    end

    % Pass the test data as dummy variable
    Dummy=Data_test;

    if Prediction_Steps==0
        % This will result immediate prediction
        dData_Es=ODE_func(0,Dummy);
        Error=norm(dData_test-dData_Es);
        Dem=norm(dData_test);
        Score=Error/Dem;
    else
        dData_Es=ODE_func(0,Dummy);
        % Get the function estimation using RK45
        for pinpin=1:Prediction_Steps
            RK_k1=dt*dData_Es;
            RK_k2=dt*ODE_func(0,Dummy+0.5*RK_k1);
            RK_k3=dt*ODE_func(0,Dummy+0.5*RK_k2);
            RK_k4=dt*ODE_func(0,Dummy+RK_k3);
            Dummy=Dummy+(1/6)*(RK_k1+2*RK_k2+2*RK_k3+RK_k4);
            dData_Es=ODE_func(0,Dummy);
        end

        % Reshape
        Dum1=reshape(dData_Es,size1,size2);
        Dum2=reshape(dData_test,size1,size2);

        % Arrange all the data to the same time
        Dum3=Dum1(1:end-Prediction_Steps,:);
        Dum4=Dum2(1+Prediction_Steps:end,:);

        % Calculate the prediction error
        Error=norm(reshape((Dum4-Dum3),[],1));
        Dem=norm(reshape(Dum4,[],1));

        % Calculate the norm
        Score=Error/Dem;
    end
else
    % Generate the function handel
    if u==0
        ODE_func=str2func(strcat('@(t,z)',name,'(t,z)'));
    end

    % Get the function estimation
    dData_Es=ODE_func(0,Data_test);
    Error=norm(dData_test-dData_Es);
    Dem=norm(dData_test);
    Score=Error/Dem;
end


%GAUSS Generation of a multivariate Gaussian dataset
%
% 	A = GAUSS(N,U,G,LABTYPE)
%
% INPUT (in case of generation a 1-class dataset in K dimensions)
%   N		    Number of objects to be generated (default 50).
%   U		    Desired mean (vector of length K).
%   G       K x K covariance matrix. Default eye(K).
%   LABTYPE Label type (default 'crisp')
%
% INPUT (in case of generation a C-class dataset in K dimensions)
%   N       Vector of length C with numbers of objects per class.
%   U       C x K matrix with class means, or
%           Dataset with means, labels and priors of classes
%           (default: zeros(C,K))
%   G       K x K x C covariance matrix of right size.
%           Default eye(K);
%   LABTYPE	Label type (default 'crisp')
%
% OUTPUT
%   A       Dataset containing multivariate Gaussian data
%
% DESCRIPTION
% Generation of N K-dimensional Gaussian distributed samples for C classes.
% The covariance matrices should be specified in G (size K*K*C) and the
% means, labels and prior probabilities can be defined by the dataset U with
% size (C*K). If U is not a dataset, it should be a C*K matrix and A will
% be a dataset with C classes.
%
% If N is a vector, exactly N(I) objects are generated for class I, I = 1..C.
%
% EXAMPLES
% 1. Generation of 100 points in 2D with mean [1 1] and default covariance
%    matrix:
%
%        GAUSS(100,[1 1])
%
% 2. Generation of 50 points for each of two 1-dimensional distributions with
%    mean -1 and 1 and with variances 1 and 2:
%
%	     GAUSS([50 50],[-1;1],CAT(3,1,2))
%
%   Note that the two 1-dimensional class means should be given as a column
%   vector [1;-1], as [1 -1] defines a single 2-dimensional mean. Note that
%   the 1-dimensional covariance matrices degenerate to scalar variances,
%   but have still to be combined into a collection of square matrices using
%   the CAT(3,....) function.
%
% 3. Generation of 300 points for 3 classes with means [0 0], [0 1] and
%    [1 1] and covariance matrices [2 1; 1 4], EYE(2) and EYE(2):
%
%      GAUSS(300,[0 0; 0 1; 1 1]*3,CAT(3,[2 1; 1 4],EYE(2),EYE(2)))
%
% SEE ALSO (PRTools Guide)
% DATASETS, PRDATASETS

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

function a = gauss(n,u,g,labtype)

		if (nargin < 1)
		prwarning (2,'number of samples not specified, assuming N = 50');
		n = 50;
		end
		cn = length(n);
	if (nargin < 2)
		prwarning (2,'means not specified; assuming one dimension, mean zero');
		u = zeros(cn,1);
	end;
	if (nargin < 3)
		prwarning (2,'covariances not specified, assuming unity');
	 	g = eye(size(u,2));
	end
	if (nargin < 4)
		prwarning (3,'label type not specified, assuming crisp');
		labtype = 'crisp';
	end

	% Return an empty dataset if the number of samples requested is 0.

	if (length(n) == 1) & (n == 0)
		a = prdataset([]);
		return
	end

	% Find C, desired number of classes based on U and K, the number of
	% dimensions. Make sure U is a dataset containing the means.

	if (isa(u,'prdataset'))
		[m,k,c] = getsize(u);
		lablist = getlablist(u);
		p = getprior(u);
		if c == 0
			u = double(u);
		end
	end
	if isa(u,'double')
		[m,k] = size(u);
		c = m;
		lablist = genlab(ones(c,1));
		u = prdataset(u,lablist);
		p = ones(1,c)/c;
	end

	if (cn ~= c) & (cn ~= 1)
		error('The number of classes specified by N and U does not match');
	end

 	% Generate a class frequency distribution according to the desired priors.
	n = genclass(n,p);

	% Find CG, the number of classes according to G.
	% Make sure G is not a dataset.

	if (isempty(g))
		g = eye(k);
		cg = 1;
	else
		g = real(+g); [k1,k2,cg] = size(g);
		if (k1 ~= k) | (k2 ~= k)
			error('The number of dimensions of the means U and covariance matrices G do not match');
		end
		if (cg ~= m & cg ~= 1)
			error('The number of classes specified by the means U and covariance matrices G do not match');
		end
	end

	% Create the data A by rotating and scaling standard normal distributions
	% using the eigenvectors of the specified covariance matrices, and adding
	% the means.

	%a = [];
  a = zeros(sum(n),k);
  nn = 0;
	for i = 1:m
		j = min(i,cg);						% Just in case CG = 1 (if G was not specified).

		% Sanity check: user can pass non-positive definite G.
		[V,D] = preig(g(:,:,j)); V = real(V); D = real(D); D = max(D,0);
    a(nn+1:nn+n(i),:) = randn(n(i),k)*sqrt(D)*V' + repmat(+u(i,:),n(i),1);
		%a = [a; randn(n(i),k)*sqrt(D)*V' + repmat(+u(i,:),n(i),1)];
    nn = nn+n(i);
	end

	% Convert A to dataset by adding labels and priors.

	labels = genlab(n,lablist);
	a = prdataset(a,labels,'lablist',lablist,'prior',p);

	% If non-crisp labels are requested, use output of Bayes classifier.
	switch (labtype)
		case 'crisp'
			;
		case 'soft'
			w = nbayesc(u,g);
			targets = a*w*classc;
			a = setlabtype(a,'soft',targets);
		otherwise
			error(['Label type ' labtype ' not supported'])
	end

	a = setname(a,'Gaussian Data');

return


function dag = sample_dag(P)
% SAMPLE_DAG Create a random directed acyclic graph with edge probabilities P(i,j)
% dag = sample_dag(P)
%
% This uses rejection sampling to reject graphs with directed cycles.

done = 0;
directed = 1;
iter = 1;
while ~done
  dag = binornd(1, P); % each edge is an indep Bernoulli (0/1) random variable
  dag = setdiag(dag, 0);
  done = acyclic(dag, directed);
  iter = iter + 1
end