% Copyright 2019, The MathWorks, Inc.
%% animateRotorPosition
%
% This script will run a Simulink model of a BLDC that is energized in
% one coil and animate the movement of the rotor. Once the animation figure
% is rendered, there is a 5 second pause before the animation begins.

close all
clear

% Below parameters are defined in the Simulink model
% Sample time
% Ts = 2e-5;
% Number of pole pairs
% p = 1;
% Initial rotor angle in degrees
% th0 = 0;
% Sector
% sector = 6;

mdl = 'Modeling_commutation_logic.slx';
open_system(mdl);

sim(mdl)
try

    %
    r = 1.2;
    theta = linspace(0,2*pi);
    x = cos(theta);
    y = sin(theta);

    x1 = 0.8*cos(theta);
    y1 = 0.8*sin(theta);

    xa = cos(0);
    ya = sin(0);

    xb = cos(2*pi/3);
    yb = sin(2*pi/3);

    xc = cos(-2*pi/3);
    yc = sin(-2*pi/3);

    xat = r*cos(0);
    yat = r*sin(0);

    xbt = r*cos(2*pi/3);
    ybt = r*sin(2*pi/3);

    xct = r*cos(-2*pi/3);
    yct = r*sin(-2*pi/3);

    hf = figure(1);h = plot(x,y,'k-',[0 xa],[0 ya],'k-',[0 xb],[0 yb],'k-',[0 xc],[0  yc],'k-',xa,ya,'ko',xb,yb,'ko',xc,yc,'ko',x1,y1,'k-');grid

    set(hf,'Color',[1 1 1])

    ha = gca;

    set(ha,'Visible','off')

    ht1 = text(xat,yat,'A');
    ht2 = text(xbt,ybt,'B');
    ht3 = text(xct,yct,'C');

    set(ht1,'FontSize',14);
    set(ht2,'FontSize',14);
    set(ht3,'FontSize',14);

    axis([-1.2 1.2 -1.2 1.2])
    axis equal

    set(h(5),'MarkerSize',20)
    set(h(6),'MarkerSize',20)
    set(h(7),'MarkerSize',20)

    switch_pattern = switchPattern.signals.values(1:40:end,:);

    init_switch_pattern = num2str(switch_pattern(1,:));

    switch init_switch_pattern

        case num2str([1 0 0 0 0 1])

            set(h(5),'MarkerFaceColor',[1 0 0])
            set(h(7),'MarkerFaceColor',[0 0 1])
            set(h(6),'MarkerFaceColor',[1 1 1])

        case num2str([0 0 1 0 0 1])

            set(h(6),'MarkerFaceColor',[1 0 0])
            set(h(7),'MarkerFaceColor',[0 0 1])
            set(h(5),'MarkerFaceColor',[1 1 1])

        case num2str([0 1 1 0 0 0])

            set(h(5),'MarkerFaceColor',[0 0 1])
            set(h(6),'MarkerFaceColor',[1 0 0])
            set(h(7),'MarkerFaceColor',[1 1 1])

        case num2str([0 1 0 0 1 0])

            set(h(5),'MarkerFaceColor',[0 0 1])
            set(h(7),'MarkerFaceColor',[1 0 0])
            set(h(6),'MarkerFaceColor',[1 1 1])

        case num2str([0 0 0 1 1 0])

            set(h(6),'MarkerFaceColor',[0 0 1])
            set(h(7),'MarkerFaceColor',[1 0 0])
            set(h(5),'MarkerFaceColor',[1 1 1])

        case num2str([1 0 0 1 0 0])

            set(h(5),'MarkerFaceColor',[1 0 0])
            set(h(6),'MarkerFaceColor',[0 0 1])
            set(h(7),'MarkerFaceColor',[1 1 1])

    end

    l1 = 0.7;
    ro = thetaSim.signals.values(1)*pi/180-pi/2;

    xr1 = l1*cos(ro);
    yr1 = l1*sin(ro);
    xr2 = -l1*cos(ro);
    yr2 = -l1*sin(ro);

    hold on,hl1 = plot([xr1 xr2],[yr1 yr2],'m-');

    set(hl1,'LineWidth',3)

    hpr = plot(xr2,yr2,'ko');
    hpb = plot(xr1,yr1,'ko');

    set(hpr,'MarkerSize',20)
    set(hpb,'MarkerSize',20)
    set(hpr,'MarkerFaceColor',[1 0 0])
    set(hpb,'MarkerFaceColor',[0 0 1])

    pause

    ro1 = thetaSim.signals.values(1:40:end)-90;

    for l = 1:numel(ro1)

        switch_pattern1 = num2str(switch_pattern(l,:));
        ro = ro1(l)*pi/180;

        xr1 = l1*cos(ro);
        yr1 = l1*sin(ro);
        xr2 = -l1*cos(ro);
        yr2 = -l1*sin(ro);

        set(hpr,'XData',xr2);
        set(hpr,'YData',yr2);
        set(hpb,'XData',xr1);
        set(hpb,'YData',yr1);

        set(hl1,'XData',[xr1 xr2]);
        set(hl1,'YData',[yr1 yr2]);

        switch switch_pattern1

            case num2str([1 0 0 0 0 1])

                set(h(5),'MarkerFaceColor',[1 0 0])
                set(h(6),'MarkerFaceColor',[1 1 1])
                set(h(7),'MarkerFaceColor',[0 0 1])

            case num2str([0 0 1 0 0 1])

                set(h(6),'MarkerFaceColor',[1 0 0])
                set(h(7),'MarkerFaceColor',[0 0 1])
                set(h(5),'MarkerFaceColor',[1 1 1])

            case num2str([0 1 1 0 0 0])

                set(h(5),'MarkerFaceColor',[0 0 1])
                set(h(6),'MarkerFaceColor',[1 0 0])
                set(h(7),'MarkerFaceColor',[1 1 1])

            case num2str([0 1 0 0 1 0])

                set(h(5),'MarkerFaceColor',[0 0 1])
                set(h(7),'MarkerFaceColor',[1 0 0])
                set(h(6),'MarkerFaceColor',[1 1 1])

            case num2str([0 0 0 1 1 0])

                set(h(6),'MarkerFaceColor',[0 0 1])
                set(h(7),'MarkerFaceColor',[1 0 0])
                set(h(5),'MarkerFaceColor',[1 1 1])

            case num2str([1 0 0 1 0 0])

                set(h(5),'MarkerFaceColor',[1 0 0])
                set(h(6),'MarkerFaceColor',[0 0 1])
                set(h(7),'MarkerFaceColor',[1 1 1])

        end

        drawnow

    end
catch

    disp("The animation was closed before the end of the simulated data")

end


function pi_values_test ( )

%*****************************************************************************80
%
%% PI_VALUES_TEST demonstrates the use of PI_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 September 2004
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PI_VALUES_TEST:\n' );
  fprintf ( 1, '  PI_VALUES returns values of\n' );
  fprintf ( 1, '  the PI function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     N         PI(N)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, fn ] = pi_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %8d  %10d\n', n, fn )

  end

  return
end


function M = stiefelstackedfactory(m, d, k)
% Stiefel(k, d)^m, represented as matrices of size m*d-by-k.
%
% function M = stiefelstackedfactory(m, d, k)
%
% Points on this manifold are matrices Y of size n x k, with n = m*d.
% Y is thought of as m matrices of size d x k each, stacked on top of each
% other. Call them Y1, ..., Ym. Each Yi is an orthonormal matrix, that is,
% its d rows are unit norm and are orthogonal to each other. Thus, this
% geometry is a product of Stiefel manifolds.
%
% To easily transform matrices Y to 3D arrays Y3 of size d x k x m such
% that each slice Y3(:, :, i) corresponds to one of the matrices Yi, use
% the functions
%
%    Y3 = M.to3D(Y)   and   Y = M.to2D(Y3).
%
% The ambient space R^(nxk) is endowed with the usual inner product
% <A, B> = trace(A'*B). This inner product is restricted to the tangent
% spaces of the present manifold, thus making it a Riemannian submanifold
% of the Euclidean space R^(nxk). Tangent vectors are represented as
% matrices of the same size as Y, and can likewise be converted to 3D
% arrays and back using to3D() and to2D().
%
% In dealing with this geometry, especially when dealing with the 3D array
% representations of points and tangent vectors, the tools multiprod,
% multitransp, multitrace, multiscale etc. available in Manopt are often
% useful.
%
% See also: stiefelfactory obliquefactory multiprod multitransp

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, May 4, 2015.
% Contributors:
% Change log:

    assert(k >= d, 'k must be at least as large as d.');

    n = m*d;

    M.name = @() sprintf('Manifold of %d orthonormal matrices of size %dx%d, stacked', m, d, k);

    M.dim = @() m*(k*d - .5*d*(d+1));

    M.size = @() [m, d, k];

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

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

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

    M.typicaldist = @() sqrt(M.dim());

    % Convert a dxkxm matrix to an nxk matrix
    M.to2D = @to2D;
    function A2 = to2D(A3)
        A2 = reshape(multitransp(A3), [k, m*d])';
    end

    % Convert an nxk matrix to a dxkxm matrix
    M.to3D = @to3D;
    function A3 = to3D(A2)
        A3 = multitransp(reshape(A2', [k, d, m]));
    end

    % Given 2 3D matrices A and B of size dxkxm, returns a 3D matrix C of
    % size dxdxm such that each slice C(:, :, i) is the symmetric part of
    % the product A(:, :, i) * B(:, :, i)'. The name is short for
    % "symmetric-block-diagonal", because if A and B were transformed to
    % their 2D equivalents via to2D, then the output would contain the
    % symmetric parts of the diagonal blocks of A*B'.
    M.symbdiag = @symbdiag;
    function C = symbdiag(A, B)
        C = multisym(multiprod(A, multitransp(B)));
    end

    % Orthogonal projection from the ambient space R^(nxk) to the tangent
    % space at X.
    M.proj = @projection;
    function Zt = projection(Y, Z)
        Y3 = to3D(Y);
        Z3 = to3D(Z);
        Lambda = symbdiag(Y3, Z3);
        Zt3 = Z3 - multiprod(Lambda, Y3);
        Zt = to2D(Zt3);
    end

    M.tangent = M.proj;

    M.egrad2rgrad = M.proj;

    M.ehess2rhess = @ehess2rhess;
    function rhess = ehess2rhess(Y, egrad, ehess, Ydot)
        Y3 = to3D(Y);
        Ydot3 = to3D(Ydot);
        egrad3 = to3D(egrad);
        C = symbdiag(Y3, egrad3);
        CYdot = to2D(multiprod(C, Ydot3));
        rhess = projection(Y, ehess - CYdot);
    end

    M.retr = @retraction;
    function Y = retraction(Y, U, t)
        if nargin < 3
            t = 1.0;
        end
        Y = Y + t*U;
        Y3 = to3D(Y);
        for i = 1 : m
            % Orthonormalize the rows of Y3(:, :, i):
            [u, s, v] = svd(Y3(:, :, i), 'econ'); %#ok<ASGLU>
            Y3(:, :, i) = u*v';
            % Alternatively, one could also use qr_unique as retraction.
        end
        Y = to2D(Y3);
    end

    M.exp = @exponential;
    function Y = exponential(Y, U, t)
        if nargin == 2
            t = 1;
        end
        tU3 = multitransp(to3D(t*U));
        Y3 = multitransp(to3D(Y));
        % From a formula by Ross Lippert, Example 5.4.2 in AMS08.
        for i = 1 : m
            X = Y3(:, :, i);
            Z = tU3(:, :, i);
            Y3(:, :, i) = [X, Z] * ...
                          expm([  X'*Z , -Z'*Z ; eye(d) , X'*Z]) * ...
                          [ expm(-X'*Z) ; zeros(d) ];
            % We may loose orthonormality here. Just to be sure:
            [u, s, v] = svd(Y3(:, :, i), 'econ'); %#ok<ASGLU>
            Y3(:, :, i) = u*v';
        end
        Y = to2D(multitransp(Y3));
    end

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

    M.rand = @random;
    function Y = random()
        Y3 = zeros(d, k, m);
        for i = 1 : m
            [Q, unused] = qr(randn(k, d), 0); %#ok<ASGLU>
            Y3(:, :, i) = Q';
        end
        Y = to2D(Y3);
    end

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

    M.lincomb = @matrixlincomb;

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

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

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

end


function MeshStat = fem_meshstats(FemFile)
% FEM_MESHSTATS: Computes and display FEM mesh volume (tetrahedral only).
%
% INPUTS:
%    - FemFile : Relative file path to a Braistorm tetrahedral FEM mesh
% OUTPUTS:
%    - MeshStat : Mtlab structure that contains all FEM mesh stqtistics (edge length, elem volum and mesh quality)
%
% DEPENDENCIES:
%    This function require the iso2mesh toolbox

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Takfarinas Medani, Francois Tadel, 2023

% Install/load iso2mesh plugin
isInteractive = 1;
[isInstalled, errInstall] = bst_plugin('Install', 'iso2mesh', isInteractive);
if ~isInstalled
    error('Plugin "iso2mesh" not available.');
end

% Get data in database
bst_progress('start', 'Mesh volume', 'Loading file...');
FemFullFile = file_fullpath(FemFile);
femmat = load(FemFullFile);
% Check type of mesh: accept only tetrahedral
if (size(femmat.Elements,2) ~= 4)
    error('This menu is available for tetrahedral meshes only.');
end
% Display results in figures if no variable in output
isDisplay = (nargout == 0);
hFig = [];

% Convert to millimeter for convenience
femmat.Vertices = 1000 .* femmat.Vertices;

% Loop over the tissues
TissueID = unique(femmat.Tissue);
for iTissue = 1:length(TissueID)
    iTissueID = find(femmat.Tissue == TissueID(iTissue));

    % 1. Edges length
    bst_progress('text', sprintf('Computing edges length...  [%d/%d]', iTissue, length(TissueID)));
    Edges = meshedge(femmat.Elements(iTissueID,:));
    n1 = femmat.Vertices(Edges(:,1),:);
    n2 = femmat.Vertices(Edges(:,2),:);
    EdgeLength = sqrt((n1(:,1)- n2(:,1)).^2 + (n1(:,2)- n2(:,2)).^2 + (n1(:,3)- n2(:,3)).^2);

    tstat.EdgeLengthMax = max(EdgeLength);
    tstat.EdgeLengthMin = min(EdgeLength);
    tstat.EdgeLengthStd = std(EdgeLength);
    tstat.EdgeLengthMean = mean(EdgeLength);
    tstat.EdgeLengthRMS = rms(EdgeLength);

    % 2. Mesh quality: Joe-Liu mesh quality metric (0-1)
    %  quality: a vector of the same length as size(elem,1), with
    %            each element being the Joe-Liu mesh quality metric (0-1) of
    %            the corresponding element. A value close to 1 represents
    %            higher mesh quality (1 means equilateral tetrahedron);
    %            a value close to 0 means nearly degenerated element.
    bst_progress('text', sprintf('Computing mesh quality...  [%d/%d]', iTissue, length(TissueID)));
    quality = 100 .*meshquality(femmat.Vertices, femmat.Elements(iTissueID,:));
    tstat.MeshQualityMax = max(quality);
    tstat.MeshQualityMin = min(quality);
    tstat.MeshQualityStd = std(quality);
    tstat.MeshQualityMean = mean(quality);

    % 3. Volume of elem
    bst_progress('text', sprintf('Computing volume of elements...  [%d/%d]', iTissue, length(TissueID)));
    voli = elemvolume(femmat.Vertices, femmat.Elements(iTissueID,:));
    tstat.MeshVolumeMax = max(voli);
    tstat.MeshVolumeMin = min(voli);
    tstat.MeshVolumeStd = std(voli);
    tstat.MeshVolumeMean = mean(voli);
    tstat.MeshVolumeSum = sum(voli);

    MeshStat.(femmat.TissueLabels{iTissue}) = tstat;

    % Visualization
    if isDisplay
        bst_progress('text', sprintf('Visualisation... [%d/%d]', iTissue, length(TissueID)));
        hFig(end+1) = figure('Name', ['Mesh stat: ' femmat.TissueLabels{iTissue}], 'NumberTitle', 'off');

        nbins = 30;
        subplot(3,1,1)
        histogram(EdgeLength,nbins);
        xlabel(sprintf('Edge length (mm):   mean=%1.2f | std=%1.2f | min=%1.2f | max=%1.2f', tstat.EdgeLengthMean, tstat.EdgeLengthStd, tstat.EdgeLengthMin, tstat.EdgeLengthMax))
        drawnow

        subplot(3,1,2)
        histogram(quality,nbins);
        xlabel(sprintf('Mesh quality (%%):   mean=%1.2f | std=%1.2f | min=%1.2f | max=%1.2f', tstat.MeshQualityMean, tstat.MeshQualityStd, tstat.MeshQualityMin, tstat.MeshQualityMax))
        drawnow

        subplot(3,1,3)
        histogram(voli,nbins);
        xlabel(sprintf('Element volume (mm3):   mean=%1.2f | std=%1.2f | min=%1.2f | max=%1.2f | sum=%1.2f', tstat.MeshVolumeMean, tstat.MeshVolumeStd, tstat.MeshVolumeMin, tstat.MeshVolumeMax, tstat.MeshVolumeSum))
        drawnow
    end
end

% For all the full Model
% 1. Edges length
bst_progress('text', 'Computing edges length...');
Edges = meshedge(femmat.Elements);
n1 = femmat.Vertices(Edges(:,1),:);
n2 = femmat.Vertices(Edges(:,2),:);
EdgeLength = sqrt((n1(:,1)- n2(:,1)).^2 + (n1(:,2)- n2(:,2)).^2 + (n1(:,3)- n2(:,3)).^2);

MeshStat.FullModel.EdgeLengthMax = max(EdgeLength);
MeshStat.FullModel.EdgeLengthMin = min(EdgeLength);
MeshStat.FullModel.EdgeLengthStd = std(EdgeLength);
MeshStat.FullModel.EdgeLengthMean = mean(EdgeLength);
MeshStat.FullModel.EdgeLengthRMS = rms(EdgeLength);

% 2. Mesh quality: Joe-Liu mesh quality metric (0-100)
bst_progress('text', 'Computing mesh quality...');
quality = 100 .* meshquality(femmat.Vertices, femmat.Elements);
MeshStat.FullModel.MeshQualityMax = max(quality);
MeshStat.FullModel.MeshQualityMin = min(quality);
MeshStat.FullModel.MeshQualityStd = std(quality);
MeshStat.FullModel.MeshQualityMean = mean(quality);

% 3. Volume of elem
bst_progress('text', 'Computing volume of elements...');
voli = elemvolume(femmat.Vertices, femmat.Elements);
MeshStat.FullModel.MeshVolumeMax = max(voli);
MeshStat.FullModel.MeshVolumeMin = min(voli);
MeshStat.FullModel.MeshVolumeStd = std(voli);
MeshStat.FullModel.MeshVolumeMean = mean(voli);
MeshStat.FullModel.MeshVolumeSum = sum(voli);

% Visualization
if isDisplay
    bst_progress('text', 'Visualisation...');
    hFig(end+1) = figure('Name', 'Mesh stat: all tissues combined', 'NumberTitle', 'off');

    nbins = 30;
    subplot(3,1,1)
    histogram(EdgeLength,nbins);
    xlabel(sprintf('Edge length (mm):   mean=%1.2f | std=%1.2f | min=%1.2f | max=%1.2f', MeshStat.FullModel.EdgeLengthMean, MeshStat.FullModel.EdgeLengthStd, MeshStat.FullModel.EdgeLengthMin, MeshStat.FullModel.EdgeLengthMax))
    drawnow

    subplot(3,1,2)
    histogram(quality,nbins);
    xlabel(sprintf('Mesh quality (%%):   mean=%1.2f | std=%1.2f | min=%1.2f | max=%1.2f', MeshStat.FullModel.MeshQualityMean, MeshStat.FullModel.MeshQualityStd, MeshStat.FullModel.MeshQualityMin, MeshStat.FullModel.MeshQualityMax))
    drawnow

    subplot(3,1,3)
    histogram(voli,nbins);
    xlabel(sprintf('Element volume (mm3):   mean=%1.2f | std=%1.2f | min=%1.2f | max=%1.2f | sum=%1.2f', MeshStat.FullModel.MeshVolumeMean, MeshStat.FullModel.MeshVolumeStd, MeshStat.FullModel.MeshVolumeMin, MeshStat.FullModel.MeshVolumeMax, MeshStat.FullModel.MeshVolumeSum))

    % Close all the figures at once
    set(hFig, 'DeleteFcn', @(h,ev)delete(setdiff(hFig,h)));
end

bst_progress('stop');


clear('pi');
y           = load('data/M3_21.dat')';
airline     = ssm_airline;
airline     = estimate(y, airline, 0.1);
theta       = -airline.param(1);
Theta12     = (-airline.param(2))^(1/12);
zetavar     = airline.param(3);

param0  = [theta+Theta12 -theta*Theta12 Theta12 Theta12 zetavar];
result4 = zeros(31, 6);
n       = 1;
% 4-5-1
for i = 1 : 6
    genair          = ssm_genair(4, 5, i);
    [genair logL]   = estimate(y, genair, param0, [], 'fmin', 'simplex', 'disp', 'off');
    result4(n, :)   = [roots([1 -genair.param(1:2)])' genair.param(3:4) reallog(genair.param(5))/2 -2*logL-13*reallog(2*pi)+2*4];
    n               = n + 1;
end
% 4-4-2
for i = 1 : 5
    for j = i+1 : 6
        genair          = ssm_genair(4, 4, [i j]);
        [genair logL]   = estimate(y, genair, param0, [], 'fmin', 'simplex', 'disp', 'off');
        result4(n, :)   = [roots([1 -genair.param(1:2)])' genair.param(3:4) reallog(genair.param(5))/2 -2*logL-13*reallog(2*pi)+2*4];
        n               = n + 1;
    end
end
% 4-3-3
for i = 2 : 5
    for j = i+1 : 6
        genair          = ssm_genair(4, 3, [1 i j]);
        [genair logL]   = estimate(y, genair, param0, [], 'fmin', 'simplex', 'disp', 'off');
        result4(n, :)   = [roots([1 -genair.param(1:2)])' genair.param(3:4) reallog(genair.param(5))/2 -2*logL-13*reallog(2*pi)+2*4];
        n               = n + 1;
    end
end
fline   = '%.5f\t\t%.5f\t\t%.5f\t\t%.5f\t\t%.5f\t\t%.5f\n';
fprintf(1, fline, result4');

param0  = [theta Theta12 Theta12 zetavar];
result3 = zeros(41, 5);
n       = 1;
% 3-5-1
for i = 1 : 6
    genair          = ssm_genair(3, 5, i);
    [genair logL]   = estimate(y, genair, param0, [], 'fmin', 'simplex', 'disp', 'off');
    result3(n, :)   = [genair.param(1:3) reallog(genair.param(4))/2 -2*logL-13*reallog(2*pi)+2*3];
    n               = n + 1;
end
% 3-4-2
for i = 1 : 5
    for j = i+1 : 6
        genair          = ssm_genair(3, 4, [i j]);
        [genair logL]   = estimate(y, genair, param0, [], 'fmin', 'simplex', 'disp', 'off');
        result3(n, :)   = [genair.param(1:3) reallog(genair.param(4))/2 -2*logL-13*reallog(2*pi)+2*3];
        n               = n + 1;
    end
end
% 3-3-3
for i = 1 : 4
    for j = i+1 : 5
        for k = j+1 : 6
            genair          = ssm_genair(3, 3, [i j k]);
            [genair logL]   = estimate(y, genair, param0, [], 'fmin', 'simplex', 'disp', 'off');
            result3(n, :)   = [genair.param(1:3) reallog(genair.param(4))/2 -2*logL-13*reallog(2*pi)+2*3];
            n               = n + 1;
        end
    end
end
fline   = '%.5f\t\t%.5f\t\t%.5f\t\t%.5f\t\t%.5f\n';
fprintf(1, fline, result3');


function [mv1, mv2, mv3] = specificIntMeanCurvDetails(img, varargin)
%SPECIFICINTMEANCURVDETAILS Ohser's Integral of Mean Curvature with details
%
%   this version is just for debugging.
%   It allows to extact contribution from each directions.
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 16/02/2005.
%

%   HISTORY

img = img~=0;

delta = [1 1 1];

% square faces of the unit cell
kr = [...
    1 2 3 4;...
    1 2 5 6;...
    1 3 5 7;...
    1 2 7 8;...
    3 5 4 6;...
    1 6 3 8;...
    2 4 5 7;...
    2 3 6 7;...
    1 5 3 8];

% triangle faces of the unit cell
kt = [...
    1 7 6;...
    2 5 8;...
    4 7 6;...
    3 5 8;...
    2 3 8;...
    4 1 7;...
    2 3 5;...
    4 1 6];

% unit surface for each cell
c1 = 0.045778;
c2 = 0.036981;
c3 = 0.035196;
c = [c1 c1 c1   c2 c2 c2   c2 c2 c2   c3 c3 c3 c3];

% distances between pixels
d1 = delta(1);
d2 = delta(2);
d3 = delta(3);
d12  = sqrt(delta(1)*delta(1) + delta(2)*delta(2));
d13  = sqrt(delta(1)*delta(1) + delta(3)*delta(3));
d23  = sqrt(delta(2)*delta(2) + delta(3)*delta(3));
s = (d12 + d23 + d13)/2;
a123 = 2*sqrt(s*(s-d12)*(s-d13)*(s-d23));

a = [d1*d2 d1*d3 d2*d3   d3*d12 d3*d12 d2*d13 d2*d13 d1*d23 d1*d23  a123 a123 a123 a123];

% compute gray-tone histogram of the image
h = grayHist(img);

mv1 = 0;
mv2 = 0;
mv3 = 0;

% for each type of configuration
for l=1:256

    v = l-1;
    b(1) = bitand(v,1)~=0;
    b(2) = bitand(v,2)~=0;
    b(3) = bitand(v,4)~=0;
    b(4) = bitand(v,8)~=0;
    b(5) = bitand(v,16)~=0;
    b(6) = bitand(v,32)~=0;
    b(7) = bitand(v,64)~=0;
    b(8) = bitand(v,128)~=0;

    % for each square face
    for nu=1:3

        b1 = b(kr(nu, 1));
        b2 = b(kr(nu, 2));
        b3 = b(kr(nu, 3));
        b4 = b(kr(nu, 4));

        s = sum([b1 b2 b3 b4]);
        if s==1
            mv1 = mv1 + h(l)*c(nu)/4/a(nu);
        elseif s==3
            mv1 = mv1 - h(l)*c(nu)/4/a(nu);
        end
    end

    % for each square diagonal face
    for nu = 4:9

        b1 = b(kr(nu, 1));
        b2 = b(kr(nu, 2));
        b3 = b(kr(nu, 3));
        b4 = b(kr(nu, 4));

        s = sum([b1 b2 b3 b4]);
        if s==1
            mv2 = mv2 + h(l)*c(nu)/4/a(nu);
        elseif s==3
            mv2 = mv2 - h(l)*c(nu)/4/a(nu);
        end
    end

    % for each triangular face
    for nu=10:13

        b1 = b(kt(nu-9, 1));
        b2 = b(kt(nu-9, 2));
        b3 = b(kt(nu-9, 3));
        b4 = b(kt(nu-5, 1));
        b5 = b(kt(nu-5, 2));
        b6 = b(kt(nu-5, 3));

        s1 = sum([b1 b2 b3]);
        s2 = sum([b4 b5 b6]);
        if s1==1
            mv3 = mv3 + h(l)*c(nu)/3/a(nu);
        end
        if s2==2
            mv3 = mv3 - h(l)*c(nu)/3/a(nu);
        end
    end
end

%imc = 4*pi*mv/sum(h(:));


domain = [-1,1,-1,1,-1,1];
% bm = 1; bp = 100;
% rx = 1; ry = 0.075; rz = 3;
% pde = elli3DorthocircIntf(bm,bp,rx,ry,rz);
%intf = @(x,y,z) x.^2+y.^2+z.^2-1;epsm = 8.85*10^(-3)*2;
epsp = 8.85*10^(-3);
sigm = 1;
sigp = 0.01;
mum = (4*pi)*3;
mup = (4*pi);
x0=0; y0=0; z0=-0.3; r1=0.2; r2=pi/5; omega = 1; a = omega*sqrt(epsp*mup); b= 150; intPt = -1;
pde = TorusTimeInitial3(mum,mup,sigm,sigp,epsm,epsp,omega,x0,y0,z0,r1,r2,a,b,intPt);

fimplicit3(pde.intf,domain)
material metal

colormap(jet)

%%%%%%%%%%%%%%%%%%%%%%%
nx = 10;  h=(domain(2) - domain(1))/nx;
ny = nx;
nz = nx;

mesh = genMesh3D(domain, nx, ny, nz);
mesh = enrichMesh3D(mesh,2); % Mesh detail level = 1 (for IFE).
mesh = genIntfMesh3D(mesh,pde.intf);
meshI = genIVmesh(mesh);

mdpt = (mesh.p(mesh.t(:,1),:) + mesh.p(mesh.t(:,2),:) +...
    mesh.p(mesh.t(:,3),:) + mesh.p(mesh.t(:,4),:))/4;
tidshow = find(mdpt(:,2)>0);
tetramesh(mesh.t(tidshow,:),mesh.p,'FaceAlpha',0)
hold on
trisurf(meshI.iface,meshI.node(:,1),meshI.node(:,2),meshI.node(:,3)) %'edgecolor','none'
box on
%axis off
axis equal
set(gca,'XTick',[],'YTick',[],'ZTick',[])

view(50,30)


function [u,p,info] =  tripreMaxwellsaddle(A,G,f,g,node,elem,bdFlag,M,grad,option)
%% tripreMaxwellsaddle solve the Maxwell system with divgence free condition,
%
%         [A  G] [u]  = [f]
%         [G' O] [p]  = [g]
%
% where  G = M_e*grad with the mass matrix for the edge element Me.
%
% This system can be rewritten as
%         [A+G*DMinv*G'       G] [u]  = f +G*DMinv*g0
%         [G'                 O] [p]  = g0
%
% where DMinv is the inverse of the diagonal matrix of the mass. Then
%
%  [A+G*DMinv*G'  G] [I    grad]                = [Abar   O  ]
%  [G'            O] [0  -Dminv*grad'*Me*grad ] = [G'     Ap]
%
%  We solve the system by GMRES with  the preconditioner
%
%       [I   grad                ]  [Abar O   ]^{-1}
%       [O  -Dminv*grad'*M_e*grad]  [G'   A_p ]
%
% where we compute the inverse Abar by mgHodgeLapE and Ap by mg.
%
% Created by Long chen and Jie Zhou on Aug,2015.

Nf = length(f);
Ng = length(g);
t = cputime;

%% Parameters
if ~exist('option','var'), option = []; end
Ndof = Nf + Ng;
option = mgoptions(option,Ndof);    % parameters
x0 = option.x0;
tol = option.tol;
maxIt = option.solvermaxit;
printlevel = option.printlevel;
d = size(node,2);

%% Set up auxiliary matrices
if d == 2
    area = simplexvolume(node,elem);
    Mvlump = accumarray([elem(:,1);elem(:,2);elem(:,3)],[area;area;area]/3,...
                        [max(elem(:)),1]);
elseif d == 3
    volume = abs(simplexvolume(node,elem)); % uniform refine in 3D is not orientation presereved
    Mvlump = accumarray([elem(:,1);elem(:,2);elem(:,3);elem(:,4)],...
                        [volume;volume;volume;volume]/4,[max(elem(:)),1]);
end
DMinv = spdiags(1./Mvlump(option.isFreeNode),0,Ng,Ng);
f = f + G*(DMinv*g);  % add second equation to the first one
Abar = A + G*DMinv*G'; % Hodge Laplacian
Ap = grad'*M*grad;    % scalar Laplacian

%% Set up matrices for multigrid
setupOption.solver = 'NO';
setupOption.isFreeEdge = option.isFreeEdge;
setupOption.freeDof = option.isFreeNode;
[~,~,Ai_N,Bi_N,BBi_N,Res_N,Pro_N] = mg(Ap,g,elem,setupOption);
[x,info,Ai,Bi,BBi,Res,Pro] = mgHodgeLapE(Abar,f,node,elem,bdFlag,setupOption); %#ok<*ASGLU>

%% Form a big matrix equation
bigA = [Abar G; G' sparse(Ng,Ng)];
bigF = [f; g];

%% Preconditioned GMRES
% options for V-cycle of Schur complement
if strcmp(option.solver,'CG') % change default set up in mgoption
   option.solver = 'Vcycle';
end
if isfield(option,'Vit')
   option.solvermaxIt = option.Vit;
else
   option.solvermaxIt = 1;
end
option.setupflag = false;
option.x0 = zeros(Nf,1);
option.printlevel = 0;
Apmgoption.printlevel = 0;
Apmgoption.printlevel = 0;
Apmgoption.setupflag = false;
Apmgoption.maxIt     = 1;
% minres for the saddle point system
[x,flag,stopErr,itStep,err] = gmres(bigA,bigF,20,tol,maxIt,@tripre,[],x0);
itStep = (itStep(1)-1)*20 + itStep(2); % total iteration
u = x(1:Nf);
p = x(Nf+1:end);

%% Output
time = cputime - t;
if printlevel >= 1
    fprintf('Triangular Preconditioned GMRES \n');
    fprintf('#dof: %8.0u,  #nnz: %8.0u, V-cycle: %2.0u, iter: %2.0u,   err = %8.2e,   time = %4.2g s\n',...
                 Ndof, nnz(bigA), option.solvermaxIt, itStep, stopErr, time)
end
if (flag == 1) && (printlevel>0)
   fprintf('NOTE: the iterative method does not converge! \n');
end
info = struct('solverTime',time,'itStep',itStep,'error',err,'flag',flag,'stopErr',stopErr);

%% Preconditioner
    function e  = tripre(r)
        r1 = r(1:Nf);
        r2 = r(Nf+1:end);
        e1 = mgHodgeLapE(Abar,r1,node,elem,bdFlag,option,Ai,Bi,BBi,Res,Pro);
        e2 = mg(Ap,r2-G'*e1,elem,Apmgoption,Ai_N,Bi_N,BBi_N,Res_N,Pro_N);
        e  = [e1 + grad*e2; -DMinv*(Ap*e2)];
    end
end


function x = nsctrec(y, dfilt, pfilt)
% NSCTREC   Nonsubsampled Contourlet Reconstruction
%
%	x = nsscrec(y, [dfilt, pfilt] )
%
% INPUT:
%   y:  a cell vector of length length(nlevs) + 1, where except y{1} is
%       the lowpass subband, each cell corresponds to one pyramidal
%       level and is a cell vector that contains bandpass directional
%       subbands from the DFB at that level.
%   dfilt:
%       a string, filter name for the directional decomposition step.
%       It is optional with default value 'dmaxflat7'. See dfilters.m for all
%       available filters.
%   pfilt:
%       a string, filter name for the pyramidal decomposition step.
%       It is optional with default value 'maxflat'. See atrousfilters.m for
%       all available filters.
%
% OUTPUT:
%   x:
%       a matrix, reconstructed image
%
% See also: ATROUSFILTERS, DFILTERS, NSCTDEC, NSFBREC, NSDFBREC

%
% HISTORY:
%     02/17/04 Created by Jianping Zhou.
%     08/07/04 Modified by Jianping Zhou. Incorporte the fast implementation
%              of convolution algorithm by Jason Laska.
%     08/30/04 Modified by Arthur. L. Cunha, added more pyramid filters.
%     10/17/04 Modified by Arthur. L. Cunha, replaced periodic with symmetric extension
%     01/24/05 Modified by Jianping Zhou, changed function names and corrected a bug.
%     10/31/05 Modified by Arthur L Cunha, corrected a bug in the pyramid decomposition (a wrong index)

% Check input
if ~exist('dfilt', 'var')
    dfilt = 'dmaxflat7' ;
end;

if ~exist('pfilt', 'var')
    pfilt = 'maxflat' ;
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Get fan filters, parallelogram filters, and pyramid filters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Get the directional filters for the critically sampled DFB.
filters = cell(4) ;
[h1, h2] = dfilters(dfilt, 'r');
% A scale is required for the nonsubsampled case.
h1 = h1./sqrt(2) ;
h2 = h2./sqrt(2) ;

% Generate the first-level fan filters by modulations.
filters{1} = modulate2(h1, 'c');
filters{2} = modulate2(h2, 'c');

% Obtain the parallelogram filters from the diamond filters
[filters{3}, filters{4}] = parafilters( h1, h2 ) ;

% Currently only one filter by Arthur Cunha
% It has been normalized.
[h1, h2, g1, g2] = atrousfilters(pfilt) ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Nonsubsampled Contourlet transform with tree structure filter banks
% Nonsubsampled pyramids make multiresolution decomposition.
% Nonsubsampled directional filter banks make directional decomposition.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
n = length(y) - 1;
xlo = y{1};
nIndex = n-1;
for i=1:n

    % Process the detail subbands
    if iscell( y{i+1} )
        % Nonsubsampled DFB reconstruction
        xhi = nsdfbrec( y{i+1}, filters );
    else
        % No DFB decomposition, copy directly
        xhi = y{i+1};
    end

    % Nonsubsampled Pyramid reconstruction
    x = nsfbrec(xlo, xhi, g1, g2, nIndex);

    % Prepare for the next level
    xlo = x ;
    nIndex = nIndex -1;
end


%% autoEncoder class
%
% Copyright (c) 2018-present, Mahmoud Afifi
% York University, Canada
% mafifi@eecs.yorku.ca | m.3afifi@gmail.com
%
% This source code is licensed under the license found in the
% LICENSE file in the root directory of this source tree.
% All rights reserved.
%
% Please cite the following work if this program is used:
% Mahmoud Afifi, Brian Price, Scott Cohen, and Michael S. Brown,
% "When color constancy goes wrong: Correcting improperly white-balanced
% images", CVPR 2019.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%

classdef autoEnc
    properties
        weights
        bias
    end
    methods
        function feature = encode(obj,hist)
            feature = (obj.sigmf(obj.weights * reshape(hist,[],1) + obj.bias,[1,0]))';
        end
        function y = sigmf(obj, x, params)
            a = cast(params(1),'like',x);
            c = cast(params(2),'like',x);
            y = 1./(1 + exp(-a*(x-c)));

        end
    end
end


%% FUNCTION Least_msmtfl_capL1
%   Multi-Stage Multi-Task Feature Learning
%
%% OBJECTIVE
%   min_W ||XW - Y||_F^2 + lambda*\sum_i \min{||W^i||_1,theta}
%
%   It's a nonconvex optimization problem, which can be relaxed into a Multi-Stage Convex optimization:
%   min_W ||XW - Y||_F^2 + \sum_i{gamma_i*||W^i||_1}
%
%% INPUT
%   X: {n * d} * t - input matrix
%   Y: {n * 1} * t - output matrix
%   lambda: regularized parameter (vector)
%   theta: theresholding paramter
%
%   (Optional)
%   opts.lFlag: estimate the upper bound of Lipschitz constant if nonzero, zero otherwise
%
%% OUTPUT
%   W: output weight
%   fun: function values
%
%% LICENSE
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as pubtolwlished 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/>.
%
%   Copyright (C) 2011 - 2012 Jiayu Zhou, Pinghua Gong and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on Dec 18, 2012.
%
%
%% RELATED PAPERS
%
% [1] Pinghua Gong, Jieping Ye, Changshui Zhang. Multi-Stage Multi-Task
%     Feature Learning. The 26th Annual Conference on Neural Information
%     Processing Systems (NIPS 2012), Lake Tahoe, Nevada, USA,
%     December 3-6, 2012.
%
%% RELATED FUNCTIONS
%  init_opts, combine_input (utils)

function [W,funcVal] = Least_msmtfl_capL1(X, Y, lambda, theta, opts)

if nargin <4
    error('\n Inputs: X, Y, and lambda1, and lambda2 should be specified!\n');
end
if nargin <5
    opts = [];
end

% initialize options.
opts=init_opts(opts);

% initial Lipschiz constant.
if isfield(opts, 'lFlag')
    lFlag = opts.lFlag;
else
    lFlag = false;
end

task_num = length(X);
[X, y, ~, samplesize] = combine_input(X, Y);
dimension = size(X, 2);

% initialize a starting point
if opts.init==2
    W0 = zeros(dimension, task_num);
elseif opts.init == 0
    W0 = randn(dimension, task_num);
else
    if isfield(opts,'W0')
        W0=opts.W0;
        if (nnz(size(W0)-[dimension, task_num]))
            error('\n Check the input .P0');
        end
    else
        W0=randn(dimension, task_num);
    end
end

% Set an array to save the objective value
funcVal = [];

W = W0;

[d,m] = size(W); % d: dimension, m: the number of tasks
X = diagonalize(X,samplesize);
XtX = X'*X; Xty = X'*y;

L1norm = max(sum(abs(X),1)); Linfnorm = max(sum(abs(X),2));

if lFlag
    % Upper bound for largest eigenvalue of Hessian matrix
    L = 2*min([L1norm*Linfnorm; size(X,1)*Linfnorm*Linfnorm; size(X,2)*L1norm*L1norm; size(X,1)*size(X,2)*max(abs(X(:)))]);
else
    % Lower bound for largest eigenvalue of Hessian matrix
    L = 2*max(L1norm*L1norm/size(X,1),Linfnorm*Linfnorm/size(X,2));
end
% Initial function value
funcVal = cat(1, funcVal, norm(X*W(:) - y)^2 + lambda*(sum(min(sum(abs(W),2),theta))));

tolw = 1e-5; % precision for inner iterations.

for iter = 1:opts.maxIter
    if iter == 1
        weight = lambda*ones(d,m);
    else
        weight = lambda*(repmat(sum(abs(W),2) < theta,1,m));
    end
    [W,~,iterw] = wLassomtl(X,y,XtX,Xty,weight,W,tolw,100,L,lFlag);

    if iterw == 1
        tolw = tolw/4;
    end

    funcVal = cat(1, funcVal, norm(X*W(:) - y)^2 + lambda*(sum(min(sum(abs(W),2),theta))));

    % stopping condition
    switch(opts.tFlag)
        case 0
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <= opts.tol)
                    break;
                end
            end
        case 1
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <=...
                        opts.tol* funcVal(end-1))
                    break;
                end
            end
        case 2
            if ( funcVal(end)<= opts.tol)
                break;
            end
        case 3
            if iter>=opts.maxIter
                break;
            end
    end

end


function [doy, year] = mydatedoy (epoch, year)
    if (nargin < 2),  year = [];  end
    if (size(epoch,2) == 1)
        % epoch is in mydatenum format
        num = epoch;
        vec = mydatevec(num);
    else
        % epoch is in mydatevec format
        vec = epoch;
        num = mydatenum(vec);
    end
    clear epoch

    %% now define the epoch corresponding to the beginning to that year:
    if isempty(year),  year = vec(:,1);  end
    if ischar(year) && any(strcmpi(year, {'median','fixed'}))
        year = median(vec(:,1));
    end
    vec0 = zeros(size(vec));
    vec0(:,1) = year;
    num0 = mydatenum(vec0);

    %%
    doy = (num - num0) ./ (3600 .* 24);
end

%!test
%! d = [2000 1 1 0 0 0];
%! doy_correct = 1;
%! doy_answer = mydatedoy(mydatenum(d));
%! myassert (doy_answer, doy_correct);

%!test
%! d = [2000 1 30 0 0 0];
%! doy_correct = 30;
%! doy_answer = mydatedoy(mydatenum(d));
%! myassert (doy_answer, doy_correct);

%!test
%! d = [2000 2 1 0 0 0];
%! doy_correct = 32;
%! doy_answer = mydatedoy(mydatenum(d));
%! myassert (doy_answer, doy_correct);


function [Ain,Cin,bin,fin] = sparse_NMF_initialization(Y,K,options) %beta,eta,X0,err_thr,max_iter)

T = size(Y,ndims(Y));
if ~ismatrix(Y)
    Y = reshape(Y,numel(Y)/T,T);
end

defoptions = CNMFSetParms;
if nargin < 3 || isempty(options); options = defoptions; end
if ~isfield(options,'snmf_max_iter'); options.snmf_max_iter = defoptions.snmf_max_iter; end
    max_iter = options.snmf_max_iter;
if ~isfield(options,'err_thr'); options.err_thr = defoptions.err_thr; end
    err_thr = defoptions.err_thr;
if ~isfield(options,'eta'); options.eta = defoptions.eta; end
    eta = options.eta*max(Y(:))^2;
if ~isfield(options,'beta'); options.beta = defoptions.beta; end
    beta = options.beta;
if ~isfield(options,'nb'), options.nb = defoptions.nb; end
    nb = options.nb;

C = rand(K,T);

repeat = 1;
iter = 1;
obj_ = 1e-10;

v = ver;
flag_optim = any(strcmp('Optimization Toolbox', {v.Name})); % check if optimization toolbox is present
if flag_optim
    if verLessThan('optim','6.3')
        min_options = optimset('Algorithm','interior-point','GradObj','On','Display','Off');
    else
        min_options = optimoptions('fmincon','Algorithm','interior-point','GradObj','On','Display','Off');
    end
end

% remove median
medY = median(Y, 2);
Y = bsxfun(@minus, Y, medY);

while (iter <= max_iter) && repeat
    A = max((Y*C')*pinv(C*C'+beta*ones(K,K)),0);
    C = max((A'*A + eta*eye(K))\(A'*Y),0);

    ff = find(sum(C,2)==0);
    if ~isempty(ff)
        A(:,ff) = [];
        C(ff,:) = [];
        K = K - length(ff);
    end

    iter = iter + 1;
    if mod(iter,10) == 0;
        A = threshold_components(A,options);
        nC = sum(C.^2,2);
        AA = A'*A;
        mine = @(e) min_e(e,beta,eta,nC,A,AA);

        e = (eta*nC./(beta*A'*sum(A,2))).^(1/4);
        if flag_optim
            e = fmincon(mine,max(e,1e-4),[],[],[],[],1e-4*ones(K,1),[],[],min_options);
        end
        C = diag(e)\C;
        A = A*diag(e);
        fprintf('%i out of maximum %i iterations done \n',iter,max_iter);
    end
    obj = norm(Y - A*C,'fro')^2 + eta*norm(C,'fro')^2 + beta*norm(sum(A,2))^2;
    repeat = abs(obj - obj_) > err_thr*obj_;
    obj_ = obj;
end

fprintf('Algorithm converged after %i iterations. \n',iter-1);

[Ain,Cin] = order_components(A,C);
[bin,fin] = nnmf(max(Y - Ain*Cin + repmat(medY,1,T),0),nb);

    function [f,grad] = min_e(e,beta,eta,nC,A,AA)
        f = eta*norm(sqrt(nC)./e)^2 + beta*norm(A*e)^2;
        grad = -2*eta*nC./(e.^3) + 2*beta*AA*e;
    end

    function [A_or,C_or] = order_components(A,C)
        nA = sqrt(sum(A.^2));
        nr = length(nA);
        A = A/spdiags(nA(:),0,nr,nr);
        mA = max(A);
        C = spdiags(nA(:),0,nr,nr)*C;
        nC2 = sqrt(sum(C.^2,2));
        mC = max(C,[],2);
        [~,srt] = sort(mC.*mA'./nC2,'descend');
        A_or = A(:,srt);
        C_or = C(srt,:);
    end

end


%DEMO_DERIVATIVEOBS  Regression problem demonstration with derivative
%                    observations
%
%  Description
%    The regression problem consist of a data with one input variable,
%    two output variables with Gaussian noise; observations and
%    derivative observations. The constructed model is full GP with
%    Gaussian likelihood.
%
%    The covariance matrix K includes now also covariances between
%    derivative observations and between derivative and latent
%    observations. With derivative observations, the K matrix is a
%    block matrix with following blocks:
%
%        K = [K_ll K_Dl'; K_Dl K_DD]
%
%    Where D refers to derivative and l to latent observation and
%       K_ll = k(x_i, x_j | th)
%       K_Dl = d k(x_i, x_j | th) / dx_i
%       K_DD = d^2 k(x_i, x_j | th) / dx_i dx_j
%
%    To include derivative observations in the inference:
%
%       - provide partial derivative observations in the
%       observation vector after output observations
%       y=[y;dy_1;...;dy_n]; for ex. if size(x)=[10 2] ->
%       size(y)=[30 1]
%
%       - gp_set(gp, 'derivobs', 'on')
%
%   The demo is organised in two parts:
%     1) data analysis without derivative observations
%     2) data analysis with derivative observations
%
%  See also  DEMO_REGRESSION1
%

% Copyright (c) 2010 Tuomas Nikoskinen

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

% Create the data
tp=9;                                  %number of training points -1
x=[-2:4/tp:2]';
y=sin(x).*cos(x).^2;                   % The underlying process
dy=cos(x).^3 - 2*sin(x).^2.*cos(x);    % Derivative of the process
ns=0.06;                               % noise standard deviation

% Add noise
y=y + ns*randn(size(y));
% derivative observations are also noisy
dy=dy + ns*randn(size(dy));
% observation vector with derivative observations
y2=[y;dy];

% test points
xt=[-3:0.05:3]';
nt=length(xt);

%========================================================
% PART 1 GP model without derivative obs
%========================================================
disp('GP model without derivative obs')

% Covariance function
pl = prior_t();
pm = prior_sqrtt();
gpcf = gpcf_sexp('lengthScale', 0.5, 'magnSigma2', .5, ...
                 'lengthScale_prior', pl, 'magnSigma2_prior', pm);
% Use default Gaussian likelihood
gp = gp_set('cf', gpcf);

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3,'DerivativeCheck','on');
% Optimize with the scaled conjugate gradient method
gp=gp_optim(gp,x,y,'opt',opt);
% Do the prediction
[Eft, Varft] = gp_pred(gp, x, y, xt);

% PLOT THE DATA

figure
%m=shadedErrorBar(p,Eft(1:size(xt)),2*sqrt(Varft(1:size(xt))),{'k','lineWidth',2});
subplot(2,1,1)
m=plot(xt,Eft,'k','lineWidth',2);
hold on
plot(xt,Eft+2*sqrt(Varft),'k--')
hold on
m95=plot(xt,Eft-2*sqrt(Varft),'k--');
hold on
hav=plot(x, y(1:length(x)), 'ro','markerSize',7,'MarkerFaceColor','r');
hold on
h=plot(xt,sin(xt).*cos(xt).^2,'b--','lineWidth',2);
%legend([m.mainLine m.patch h hav],'prediction','95%','f(x)','observations');
legend([m m95 h hav],'prediction','95%','f(x)','observations');
title('GP without derivative observations')
xlabel('input x')
ylabel('output y')

%========================================================
% PART 2 GP model with derivative obs
%========================================================
disp('GP model with derivative obs')

% Option derivobs set so that the derivatives are in use
gp = gp_set('cf', gpcf, 'derivobs', 'on');

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3,'DerivativeCheck','on');
% Optimize with the scaled conjugate gradient method
gp=gp_optim(gp,x,y2,'opt',opt);
% Do the prediction
[Eft2, Varft2] = gp_pred(gp, x, y2, xt);
% Use predictions for function values only
Eft2=Eft2(1:nt);Varft2=Varft2(1:nt);

% PLOT THE DATA
% plot lines indicating the derivative

subplot(2,1,2)
m=plot(xt,Eft2,'k','lineWidth',2);
hold on
plot(xt,Eft2+2*sqrt(Varft2),'k--')
hold on
m95=plot(xt,Eft2-2*sqrt(Varft2),'k--');
hold on
hav=plot(x, y(1:length(x)), 'ro','markerSize',7,'MarkerFaceColor','r');
hold on
h=plot(xt,sin(xt).*cos(xt).^2,'b--','lineWidth',2);

xlabel('input x')
ylabel('output y')
title('GP with derivative observations')

i1=0;
a=0.1;
ddx=zeros(2*length(x),1);
ddy=zeros(2*length(x),1);
for i=1:length(x)
  i1=i1+1;
  ddx(i1)=x(i)-a;
  ddy(i1)=y(i)-a*dy(i);
  i1=i1+1;
  ddx(i1)=x(i)+a;
  ddy(i1)=y(i)+a*dy(i);
end

for i=1:2:length(ddx)
  hold on
  dhav=plot(ddx(i:i+1), ddy(i:i+1),'r','lineWidth',2);
end
legend([m m95 h hav dhav],'prediction','95%','f(x)','observations','der. obs.');


function [d] = triangle2distance(tri, pos, s, maxinitdist)

% TRIANGLE2DISTANCE computes the geodesic distance (across the edges) on a
% mesh, using Dijkstra's algorithm. The Dijkstra code is an efficient
% vectorized version of a function from MIT's graphtool toolbox, operating
% on an adjacency matrix.
%
% Use as
%   d = triangle2distance(tri, pos, s)
%
% Input arguments:
%   tri = Mx3 matrix describing the triangles
%   pos = Nx3 matrix describing the position of the vertices
%   s   = (can be empty), scalar or vector with indices for the points for
%         which the distance (to all other points) will be computed. If
%         empty or not defined, all points will be considered.
%
% Output argument:
%   d   = Nxnumel(s) distance matrix

% Copyright (C) 2015, Jan-Mathijs Schoffelen
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id:$

adj = triangle2connectivity(tri, pos);
n   = length(adj);
if nargin<3 || isempty(s)
  s  = 1:n;
end
if nargin<4
  maxinitdist = inf;
end
ns = length(s);

d    = inf*ones(n,ns); % distance s-all nodes
for k = 1:ns
  d(s(k),k) = 0; % s-s distance
end

for k = 1:ns
  if mod(k,10)==0,
    fprintf('computing distance between node %d and nodes 1:%d\n', s(k), n);
  end
  T = 1:n;    % node set with shortest paths not found

  if isfinite(maxinitdist)
    initd = sqrt(sum((pos-pos(s(k),:)).^2,2));
    T(initd>=maxinitdist) = [];
  end

  %while ~isempty(T) %%% a for-loop goes ~10% faster, and we know how often
  %we need to iterate
  for m = 1:n
    if mod(m,1000)==0,
      fprintf('looping across vertices %d/%d\n', m, n);
    end
    [dmin,ind] = min(d(T,k));

    adj_ = adj(T,T(ind));
    d_   = d(T,k);

    % logic: shrink the distance if there's an edge (adj_>0) AND if it's
    % shorter to travel through this edge
    criterion = adj_ > 0 & d_ > d(T(ind),k)+adj_;
    d(T(criterion),k) = d(T(ind),k) + adj_(criterion);

    T(ind) = [];
  end
end

% the below code is from MIT's graphtool toolbox. the above code that
% computes the distance based on an adjacency matrix is taken from there,
% but the vectorized functionality is ~10 times as fast.

% Implements a simple version of the Dijkstra shortest path algorithm
% Returns the distance from a single vertex to all others, doesn't save the path
% INPUTS: adjacency matrix (adj), start node (s)
% OUTPUTS: shortest path length from start node to all other nodes
% Note: works with a weighted/directed matrix
% GB, Last Updated: December 13, 2004

% Copyright (c) 2011, Massachusetts Institute of Technology.
% All rights reserved.
% Redistribution and use in source and binary forms, with or without modification,
% are permitted provided that the following conditions are met:
%
%    Redistributions of source code must retain the above copyright notice, this list
%    of conditions and the following disclaimer.
%    Redistributions in binary form must reproduce the above copyright notice, this list
%    of conditions and the following disclaimer in the documentation and/or other materials
%    provided with the distribution.
%    Neither the name of the Massachusetts Institute of Technology nor the names of its
%    contributors may be used to endorse or promote products derived from this software without
%    specific prior written permission.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
% IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
% FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
% CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
% DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
% DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
% IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
% THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

% function d = simple_dijkstra(adj,s)
%
% n=length(adj);
% d = inf*ones(1,n); % distance s-all nodes
% d(s) = 0;    % s-s distance
% T = 1:n;    % node set with shortest paths not found
%
% while not(isempty(T))
%     [dmin,ind] = min(d(T));
%     for j=1:length(T)
%         if adj(T(ind),T(j))>0 & d(T(j))>d(T(ind))+adj(T(ind),T(j))
%             d(T(j))=d(T(ind))+adj(T(ind),T(j));
%         end
%     end
%     T = setdiff(T,T(ind));
%
% end


function [y, dh_struct] = findgrad(X_struct, f, e, varargin)

[X_vec, X_template] = struct2vector(X_struct);

[y] = feval(f, X_struct, varargin{:});

dh = nan(length(X_vec),1) ;

for j = 1:length(X_vec)

  dx = zeros(length(X_vec),1);
  dx(j) = dx(j) + e;

  [y2] = feval(f, vector2struct(X_vec + dx, X_template), varargin{:});
  [y1] = feval(f, vector2struct(X_vec - dx, X_template), varargin{:});

  dh(j) = (y2 - y1)/(2*e);
end

dh_struct = vector2struct(dh, X_template);


function [SeqR1s,SeqR5s]=MADAll(fea_Train,gnd_Train,fea_Test,gnd_Test,testDims,wgts)
%Nearest neighbor classifier with MAD measure and symmetric matching

nTrain = size(fea_Train,1);
nTest = size(fea_Test,1);
nDim=length(testDims);%number of feature dimensions to test
SeqR1s=zeros(nDim,1);%Rank 1 recognition rate based on matching gait sequences
SeqR5s=zeros(nDim,1);%Rank 5 recognition rate based on matching gait sequences

for iDim=1:nDim
    Dim=testDims(iDim);
    feaTrn=fea_Train(:,1:Dim);
    feaTst=fea_Test(:,1:Dim);
    DMat=MAD(feaTrn,feaTst, wgts);%This is the MAD distance matrix
    [SeqR1s(iDim,1),SeqR5s(iDim,1)]=SeqDist(DMat,gnd_Train,gnd_Test);
end

%Calculate matching scores between each training/test sample pair
function D = MAD(feaTrn, feaTst, wgts)
if size(wgts,2)==1, wgts=wgts';end
[numTst,p] = size(feaTst);
numTrn = size(feaTrn,1);
wgts=wgts(1:p);
D = zeros(numTst,numTrn);
for i=1:numTst
    for j=1:numTrn
        A=feaTst(i,:);
        B=feaTrn(j,:);
        % the following two lines calculate the MAD between A and B
        dist=sum(sum(sum(A.*B./wgts)));
        D(i,j)=-dist/(norm(A(:))*norm(B(:)));
    end
end

%The recognition rate between two sequences
function [SeqR1,SeqR5]=SeqDist(DMat,gnd_Train,gnd_Test)
trnSeqs=unique(gnd_Train);
numTrnSeq=length(trnSeqs);

tstSeqs=unique(gnd_Test);
numTstSeq=length(tstSeqs);

SeqDMat=zeros(numTstSeq,numTrnSeq);
for i=1:numTstSeq
    idxs_i=find(gnd_Test==tstSeqs(i));
    iMat=DMat(idxs_i,:);
    for j=1:numTrnSeq
        idxs_j=find(gnd_Train==trnSeqs(j));
        jMat=iMat(:,idxs_j);
        SeqDMat(i,j)=mean(min(jMat,[],1))+mean(min(jMat,[],2));
    end
end
[minDs,minIdxs]=min(SeqDMat, [], 2);%Minimum distance implies best match
IDs=trnSeqs(minIdxs);
SeqR1=sum(IDs==tstSeqs)/numTstSeq;%Rank 1

[stDs,stIdxs]=sort(SeqDMat,2);
IDs=trnSeqs(stIdxs(:,1:5));
tstSeqs5=repmat(tstSeqs,1,5);
SeqR5=sum(sum(IDs==tstSeqs5))/numTstSeq;%Rank 5


function beta = lars(X, y, method, stop, useGram, Gram, Cardi, bSparse, trace)
% This function is provided at
% http://www2.imm.dtu.dk/pubdb/views/publication_details.php?id=3897
% I have made some small modifications  -- Deng Cai, Feb/2008

% LARS  The LARS algorithm for performing LAR or LASSO.
%    BETA = LARS(X, Y) performs least angle regression on the variables in
%    X to approximate the response Y. Variables X are assumed to be
%    normalized (zero mean, unit length), the response Y is assumed to be
%    centered.
%    BETA = LARS(X, Y, METHOD), where METHOD is either 'LARS' or 'LASSO'
%    determines whether least angle regression or lasso regression should
%    be performed.
%    BETA = LARS(X, Y, METHOD, STOP) with nonzero STOP will perform least
%    angle or lasso regression with early stopping. If STOP is negative,
%    STOP is an integer that determines the desired number of variables. If
%    STOP is positive, it corresponds to an upper bound on the L1-norm of
%    the BETA coefficients.
%    BETA = LARS(X, Y, METHOD, STOP, USEGRAM) specifies whether the Gram
%    matrix X'X should be calculated (USEGRAM = 1) or not (USEGRAM = 0).
%    Calculation of the Gram matrix is suitable for low-dimensional
%    problems. By default, the Gram matrix is calculated.
%    BETA = LARS(X, Y, METHOD, STOP, USEGRAM, GRAM) makes it possible to
%    supply a pre-computed Gram matrix. Set USEGRAM to 1 to enable. If no
%    Gram matrix is available, exclude argument or set GRAM = [].
%    BETA = LARS(X, Y, METHOD, STOP, USEGRAM, GRAM, TRACE) with nonzero
%    TRACE will print the adding and subtracting of variables as all
%    LARS/lasso solutions are found.
%    Returns BETA where each row contains the predictor coefficients of
%    one iteration. A suitable row is chosen using e.g. cross-validation,
%    possibly including interpolation to achieve sub-iteration accuracy.
%
% Author: Karl Skoglund, IMM, DTU, kas@imm.dtu.dk
% Reference: 'Least Angle Regression' by Bradley Efron et al, 2003.

%% Input checking
% Set default values.
if nargin < 9
    trace = 0;
end
if nargin < 8
    bSparse = 1;
end
if nargin < 7
    Cardi = [];
end
if nargin < 6
    Gram = [];
end
if nargin < 5
    useGram = 0;
end
if nargin < 4
    stop = 0;
end
if nargin < 3
    method = 'lasso';
end
if strcmpi(method, 'lasso')
    lasso = 1;
else
    lasso = 0;
end

if isempty(X)
    error('The code has been updated. Please input the X');
end

%% LARS variable setup
[n p] = size(X);
% nvars = min(n-1,p); %
nvars = p; %

maxk = 512*nvars; % Maximum number of iterations

if isempty(Cardi)
    if stop == 0
        if bSparse
            beta = sparse(p,2*nvars);
        else
            beta = zeros(p,2*nvars);
        end
    elseif stop < 0
        if bSparse
            beta = sparse(p,2*round(-stop));
        else
            beta = zeros(p,2*round(-stop));
        end
    else
        if bSparse
            beta = sparse(p,100);
        else
            beta = zeros(p,100);
        end
    end
else
    Cardi = unique(Cardi);
    Cardi(Cardi>nvars) = [];
    stop = -max(Cardi);
    if bSparse
        beta = sparse(p,length(Cardi));
    else
        beta = zeros(p,length(Cardi));
    end
    betak = zeros(p,1);
end

mu = zeros(n, 1); % current "position" as LARS travels towards lsq solution
I = 1:p; % inactive set
A = []; % active set

% Calculate Gram matrix if necessary
if isempty(Gram) && useGram
    error('The code has been updated. Please input the Gram');
%     clear Gram;
%     global Gram;
    %   Gram = X'*X; % Precomputation of the Gram matrix. Fast but memory consuming.
end

if ~useGram
    R = []; % Cholesky factorization R'R = X'X where R is upper triangular
end

lassocond = 0; % LASSO condition boolean
stopcond = 0; % Early stopping condition boolean
k = 0; % Iteration count
vars = 0; % Current number of variables

if trace
    disp(sprintf('Step\tAdded\tDropped\t\tActive set size'));
end

% TimeLoop = zeros(2*nvars,1);
tmpT = cputime;

%% LARS main loop
while vars < nvars && ~stopcond && k < maxk
    k = k + 1;
    c = X'*(y - mu);
    [C j] = max(abs(c(I)));
    j = I(j);

    if ~lassocond % if a variable has been dropped, do one iteration with this configuration (don't add new one right away)
        if ~useGram
            diag_k = X(:,j)'*X(:,j); % diagonal element k in X'X matrix
            if isempty(R)
                R = sqrt(diag_k);
            else
                col_k = X(:,j)'*X(:,A); % elements of column k in X'X matrix
                R_k = R'\col_k'; % R'R_k = (X'X)_k, solve for R_k
                R_kk = sqrt(diag_k - R_k'*R_k); % norm(x'x) = norm(R'*R), find last element by exclusion
                R = [R R_k; [zeros(1,size(R,2)) R_kk]]; % update R
            end
        end
        A = [A j];
        I(I == j) = [];
        vars = vars + 1;
        if trace
            disp(sprintf('%d\t\t%d\t\t\t\t\t%d', k, j, vars));
        end
    end

    s = sign(c(A)); % get the signs of the correlations

    if useGram
        if vars <= 200
            R = chol(Gram(A,A));
        elseif lassocond
            if (rJ <= 200) & vars <= 1000
                R = chol(Gram(A,A));
            else
                R(:,rJ) = []; % remove column j
                tmpn = size(R,2);
                for tmpk = rJ:tmpn
                    tmpp = tmpk:tmpk+1;
                    [G,R(tmpp,tmpk)] = planerot(R(tmpp,tmpk)); % remove extra element in column
                    if tmpk < tmpn
                        R(tmpp,tmpk+1:tmpn) = G*R(tmpp,tmpk+1:tmpn); % adjust rest of row
                    end
                end
                R(end,:) = []; % remove zero'ed out row
            end
        else
            R_k = R'\Gram(A(1:end-1),j);
            R_kk = sqrt(Gram(j,j)-R_k'*R_k);
            R = [R R_k; [zeros(1,size(R,2)) R_kk]]; % update R
        end
        GA1 = R\(R'\s);
        AA = 1/sqrt(sum(GA1.*s));
        w = AA*GA1;
    else
        GA1 = R\(R'\s);
        AA = 1/sqrt(sum(GA1.*s));
        w = AA*GA1;
    end
    u = X(:,A)*w; % equiangular direction (unit vector)

    if vars == nvars % if all variables active, go all the way to the lsq solution
        gamma = C/AA;
    else
        a = X'*u; % correlation between each variable and eqiangular vector
        temp = [(C - c(I))./(AA - a(I)); (C + c(I))./(AA + a(I))];
        gamma = min([temp(temp > 0); C/AA]);
    end

    % LASSO modification
    if lasso
        lassocond = 0;
        if isempty(Cardi)
            temp = -beta(A,k)./w;
        else
            temp = -betak(A)./w;
        end
        [gamma_tilde] = min([temp(temp > 0); gamma]);
        j = find(temp == gamma_tilde);
        if gamma_tilde < gamma,
            gamma = gamma_tilde;
            lassocond = 1;
        end
    end

    mu = mu + gamma*u;
    if isempty(Cardi)
        if size(beta,2) < k+1
            if bSparse
                beta = [beta sparse(p,size(beta,1))];
            else
                beta = [beta zeros(p,size(beta,1))];
            end
        end
        beta(A,k+1) = beta(A,k) + gamma*w;
    else
        tmpbetak = betak(A) + gamma*w;
        betak = zeros(p,1);
        betak(A) = tmpbetak;
        idx = find(Cardi==vars);
        if ~isempty(idx)
            beta(:,idx) = betak;
        end
    end

    % Early stopping at specified bound on L1 norm of beta
    if isempty(Cardi)
        if stop > 0
            t2 = sum(abs(beta(:,k+1)));
            if t2 >= stop
                t1 = sum(abs(beta(:,k)));
                s = (stop - t1)/(t2 - t1); % interpolation factor 0 < s < 1
                beta(:,k+1) = beta(:,k) + s*(beta(:,k+1) - beta(:,k));
                stopcond = 1;
            end
        end
    end

    % If LASSO condition satisfied, drop variable from active set
    if lassocond == 1
        if ~useGram
            R(:,j) = []; % remove column j
            tmpn = size(R,2);
            for tmpk = j:tmpn
                tmpp = tmpk:tmpk+1;
                [G,R(tmpp,tmpk)] = planerot(R(tmpp,tmpk)); % remove extra element in column
                if tmpk < tmpn
                    R(tmpp,tmpk+1:tmpn) = G*R(tmpp,tmpk+1:tmpn); % adjust rest of row
                end
            end
            R(end,:) = []; % remove zero'ed out row
        end
        rJ = j;
        I = [I A(j)];
        A(j) = [];
        vars = vars - 1;
        if trace
            disp(sprintf('%d\t\t\t\t%d\t\t\t%d', k, j, vars));
        end
    end

    % Early stopping at specified number of variables
    if stop < 0
        stopcond = vars >= -stop;
    end

%     TimeLoop(k) = cputime - tmpT;
%     tmpT = cputime;

%     if vars < 1000
%         if mod(vars,500) == 0
%             tmpT = cputime - tmpT;
%             disp(['LARS: ',num2str(vars),' features selected. Time: ',num2str(tmpT)]);
%             tmpT = cputime;
%         end
%     elseif vars < 2000
%         if mod(vars,200) == 0
%             tmpT = cputime - tmpT;
%             disp(['LARS: ',num2str(vars),' features selected. Time: ',num2str(tmpT)]);
%             tmpT = cputime;
%         end
%     elseif vars < 3000
%         if mod(vars,100) == 0
%             tmpT = cputime - tmpT;
%             disp(['LARS: ',num2str(vars),' features selected. Time: ',num2str(tmpT)]);
%             tmpT = cputime;
%         end
%     else
%         if mod(vars,50) == 0
%             tmpT = cputime - tmpT;
%             disp(['LARS: ',num2str(vars),' features selected. Time: ',num2str(tmpT)]);
%             tmpT = cputime;
%         end
%     end
end

if isempty(Cardi)
    % trim beta
    if size(beta,2) > k+1
        beta(:,k+2:end) = [];
    end
end

if k == maxk
    disp('LARS warning: Forced exit. Maximum number of iteration reached.');
end

%% To do
%
% There is a modification that turns least angle regression into stagewise
% (epsilon) regression. This has not been implemented.


function ll = mlpLogLikelihood(model)

% MLPLOGLIKELIHOOD Multi-layer perceptron log likelihood.
% FORMAT
% DESC computes the log likelihood of a multi-layer perceptron
% model. For single hidden layer models this is done by wrapping
% the mlperr command.
% ARG model : the model structure for computing the log likelihood.
% RETURN ll : the model log likelihood.
%
% SEEALSO : modelLogLikeihood, mlperr
%
% COPYRIGHT : Neil D. Lawrence, 2006, 2007

% MLTOOLS

if length(model.hiddenDim) == 1
  ll = -mlperr(model, model.X, model.y);
else
  Y = mlpOut(model, model.X);
  ll = -0.5*sum(sum((model.Y - Y).^2));
end

ll = ll - size(model.X, 1)/2*log(2*pi);


% Test size bounds with varying mesh gradation rates.
clearvars; clc;

addpath('..')
addpath(genpath('../utilities/'))
addpath(genpath('../datasets/'))
addpath(genpath('../m_map/'))

RESO_TOL = 95; %percentage of resolution in bounds

bbox = [166 176;		% lon_min lon_max
    -48 -40]; 		% lat_min lat_max
min_el    = 1e3;  		% minimum resolution in meters.
max_el    = 100e3; 		% maximum resolution in meters.
max_el_ns = 5e3;        % maximum resolution nearshore in meters.
grade     = [0.15; 0.25; 0.35]; 		% mesh grade in decimal percent.
R         = 3;    		% number of elements to resolve feature width.
coastline = 'GSHHS_f_L1';
gdat = geodata('shp',coastline,'bbox',bbox,'h0',min_el);

for i = 1 : 3 % for each grade
    fh = edgefx('geodata',gdat,...
        'fs',R,'max_el_ns',max_el_ns,...
        'max_el',max_el,'g',grade(i));
    mshopts = meshgen('ef',fh,'bou',gdat,'plot_on',0,'nscreen',5,'proj','trans');
    mshopts = mshopts.build;
    m1 = mshopts.grd;

    [bars,barlen] = GetBarLengths(m1,0);
    % sort bar lengths in descending order
    [barlen,IA] = sort(barlen,'descend');
    bars = bars(IA,:);
    % get the minimum bar length for each node
    [B1,IB] = unique(bars(:,1),'last');
    [B2,IC] = unique(bars(:,2),'last');
    d1 = NaN*m1.p(:,1); d2 = NaN*m1.p(:,1);
    d1(B1) = barlen(IB); d2(B2) = barlen(IC);
    reso = min(d1,d2);

    reso_in_bounds = 100*sum(reso > min_el & reso < max_el)/length(reso);
    if reso_in_bounds < RESO_TOL
        error(['Resolution bounds does not match for grade ' num2str(grade(i)) ...
               '. Got ' num2str(reso_in_bounds) '% of vertices with resolution in bounds']);
        exit(1)
    end
    disp(['Passed for ' num2str(grade(i)) '. ' ...
          num2str(reso_in_bounds) '% of vertices have resolution in bounds']);
end


function [amppts1,timepts,okflag] = ISM_RIRpow_approx(aa,room,cc,timepts,rt_type,rt_val)
%ISM_RIRpow_approx  Approximation of ISM RIR power (Lehmann & Johansson's method)
%
% [P_VEC,T_VEC,OK_FLAG] = ISM_RIRpow_approx(ALPHA,ROOM,C,T_VEC,RT_TYPE,RT_VAL)
%
% This function returns the predicted values of RIR power in P_VEC (as
% would result from ISM simulations) estimated by means of the EDC
% approximation method described in: "Prediction of energy decay in room
% impulse responses simulated with an image-source model", J. Acoust. Soc.
% Am., vol. 124(1), pp. 269-277, July 2008. The values of P_VEC are
% computed for the time points given as input in T_VEC (in sec), which is
% assumed to contain increasing values of time. The vector T_VEC (and
% corresponding vector P_VEC) will be cropped if the numerical computation
% limits are reached for the higher time values in T_VEC (for which NaNs
% are generated in P_VEC), in which case the output parameter OK_FLAG will
% be set to 0 (1 otherwise).
%
% The environmental setting is defined via the following input parameters:
%
%    ALPHA: 1-by-6 vector, corresponding to each wall's absorption
%           coefficient: [x1 x2 y1 y2 z1 z2]. Index 1 indicates wall closest
%           to the origin. E.g.: [0.5 0.5 0.45 0.87 0.84 0.32].
%  RT_TYPE: character string, measure of reverberation time used for the
%           definition of the coefficients in ALPHA. Set to either 'T60' or
%           'T20'.
%   RT_VAL: scalar, value of the reverberation time (in seconds) defined by
%           RT_TYPE. E.g.: 0.25.
%     ROOM: 1-by-3 vector, indicating the rectangular room dimensions
%           (in m): [x_length y_length z_length]. E.g.: [4 4 3].
%        C: scalar (in m/s), propagation speed of sound waves. E.g.: 343.

% Release date: November 2009
% Author: Eric A. Lehmann, Perth, Australia (www.eric-lehmann.com)
%
% Copyright (C) 2009 Eric A. Lehmann
%
% 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/>.

numradpts = length(timepts);
radpts = cc * timepts;              % radius values corresponding to time points

bxx = ( sqrt(1-aa(1))*sqrt(1-aa(2)) )^(1/room(1));
byy = ( sqrt(1-aa(3))*sqrt(1-aa(4)) )^(1/room(2));
bzz = ( sqrt(1-aa(5))*sqrt(1-aa(6)) )^(1/room(3));

if bxx==byy && byy==bzz,
    intcase = 1;
elseif bxx==byy && bxx~=bzz,
    intcase = 2;
elseif byy==bzz && bzz~=bxx,
    if bzz<bxx,     % coordinate swap x<->z
        foo = bxx; bxx = bzz; bzz = foo;
        intcase = 2;
    else
        intcase = 3;
    end
elseif bxx==bzz && bzz~=byy,
    if bzz<byy,     % coordinate swap y<->z
        foo = byy; byy = bzz; bzz = foo;
        intcase = 2;
    else
        intcase = 4;
    end
else
    intcase = 5;
    if bxx>bzz && bxx>byy,      % coordinate swap x<->z
        foo = bxx; bxx = bzz; bzz = foo;
    elseif byy>bzz && byy>bxx,	% coordinate swap y<->z
        foo = byy; byy = bzz; bzz = foo;
    end
end

amppts1 = zeros(1,numradpts);
for ss=1:numradpts,    % compute amplitude/energy estimates
    Bx = bxx^(radpts(ss)); Bx(Bx==0) = eps;
    By = byy^(radpts(ss)); By(By==0) = eps;
    Bz = bzz^(radpts(ss)); Bz(Bz==0) = eps;
    switch intcase
        case 1
            int2 = Bx;
        case 2
            int2 = (Bx-Bz) / log(Bx/Bz);
        case 3
            n1 = log(Bz/Bx);
            int2 = Bz*( expint(n1) + log(n1) + 0.5772156649 ) / n1;
        case 4
            n1 = log(Bz/By);
            int2 = Bz*( expint(n1) + log(n1) + 0.5772156649 ) / n1;
        otherwise
            n1 = log(Bz/By);
            n2 = log(Bz/Bx);
            int2 = Bz*(log(n1/n2) + expint(n1) - expint(n2)) / log(Bx/By);
    end
    amppts1(ss) = int2/radpts(ss);      % 'propto' really...
end

okflag = 1;
foo = find(isnan(amppts1),1);
if ~isempty(foo),
    amppts1 = amppts1(1:foo-1);
    timepts = timepts(1:foo-1);
    okflag = 0;
end

if nargin==6,
    switch lower(rt_type)  % offset correction
        case 't60', sl = exp(3.05*exp(-1.85*rt_val));
        case 't20', sl = exp(3.52*exp(-7.49*rt_val));
    end
    amppts1 = amppts1 ./ exp(sl*(timepts-timepts(1)));
end


close all;
clear all;
clc;
%rng('default');
% Create the directory for storing images
[status_code,message,message_id] = mkdir('bin');

% Signal space
N = 1000;
% Number of measurements
M = 200;
K = 50;

omp_success = 0;
mc_omp_success = 0;
for nt=1:100
    % Sensing matrix
    Phi = spx.dict.simple.gaussian_dict(M, N);
    % Construct the signal generator.
    gen  = spx.data.synthetic.SparseSignalGenerator(N, K);
    % Generate bi-uniform signals
    x = gen.gaussian;
    % Measurement vectors
    y = Phi.apply(x);

    % OMP solver instance
    solver = spx.pursuit.single.OrthogonalMatchingPursuit(Phi, K);
    % Solve the sparse recovery problem
    omp_result = solver.solve(y);
    % Solution vector
    z = omp_result.z;
    omp_stats = spx.commons.sparse.recovery_performance(Phi, K, y, x, z);
    %fprintf('OMP\n');
    %spx.commons.sparse.print_recovery_performance(omp_stats);

    % MC-OMP solver instance
    solver = spx.pursuit.single.MC_OMP(Phi, K);
    %solver.BranchingFactor = 4;
    %solver.MaxCandidatesToRetain = 8;
    % Solve the sparse recovery problem
    mcomp_result = solver.solve(y);
    % Solution vector
    z = mcomp_result.z;
    mcomp_stats = spx.commons.sparse.recovery_performance(Phi, K, y, x, z);
    %fprintf('MC-OMP\n');
    %spx.commons.sparse.print_recovery_performance(mcomp_stats);

    fprintf('K=%d, Trial: %d, OMP: %s, MC-OMP: %s\n', ...
        K, nt, spx.io.true_false_short(omp_stats.success), ...
        spx.io.true_false_short(mcomp_stats.success));
    omp_success = omp_success  + omp_stats.success;
    mc_omp_success = mc_omp_success  + mcomp_stats.success;
end
fprintf('TOTAL: OMP : %d, MC-OMP: %d\n', omp_success, mc_omp_success);


% sin_x.m: This m-file calculates and plots the
% function sin(x) for 0 <= x <= 6.
x = 0:0.1:6;
y = sin(x);
plot(x,y);


function rowMatrix = computeRowsOfToeplitzHankelMatrix(rowNumber,...
        nColumns, crossCorrelationVectors, hankelMatrixIsAdded, dcIsIncluded)
%     if rowNumber == 1
%         toeplitzRows = crossCorrelationVectors(1:nColumns,:);
%     else
%         toeplitzRows = ...
%             [flip(crossCorrelationVectors(2:rowNumber,:),1);...
%             crossCorrelationVectors(1:nColumns-rowNumber+1,:)];
%     end
% modified by Liming Shi for fast computation in MATLAB
    toeplitzRows=crossCorrelationVectors(rowNumber:-1:1,:);

    if dcIsIncluded && hankelMatrixIsAdded
        hankelOffset = 1;
    else
        hankelOffset = 3;
    end

    hankelRows = crossCorrelationVectors((0:nColumns-1)+...
                                         hankelOffset+rowNumber-1,:);

    if hankelMatrixIsAdded
        rowMatrix = toeplitzRows + hankelRows;
    else
        rowMatrix = toeplitzRows - hankelRows;
    end

end


function f = spp_scale_features(f, feat_norm_mean)
% My initial experiments were conducted on features with an average norm
% very close to 20. Using those features, I determined a good range of SVM
% C values to cross-validate over. Features from different layers end up
% have very different norms. We rescale all features to have an average norm
% of 20 (why 20? simply so that I can use the range of C values found in my
% initial experiments), to make the same search range for C reasonable
% regardless of whether these are pool5, fc6, or fc7 features. This strategy
% seems to work well. In practice, the optimal value for C ends up being the
% same across all features.
target_norm = 20;
f = f .* (target_norm / feat_norm_mean);


function kernel = create_kernel(kernel_type, pars, nMax, lb, ub, bound_pars)
%% create convolution kernel
%% inputs:
%   kernel_type: string, convolution kernel type. now support {'exp',
%       'exp2', 'vector'}
%   pars: parameters for the selected kernel type
%   nMax: length of the kernel
%   lb:     lower bound for each parameter
%   ub:     upper bound for each parameter
%   bound_pars: logical variable, bound the parameters or not {1, 0}
%% outputs
%   kernel: struct variable

%% Author: Pengcheng Zhou, Carnegie Mellon University, 2016

%% kernel size
if ~exist('nMax', 'var') || isempty(nMax)
    nMax = 50;
end
kernel.nMax = nMax;

%% initialize kernel
if ~exist('kernel_type', 'var') || isempty(kernel_type)
    kernel_type = 'exp2';
end
if strcmpi(kernel_type, 'exp')
    % single exponential function: ~ exp(-t/tau)
    kernel.type = 'exp';
    % parameter
    if ~exist('pars', 'var') || isempty(pars)
        kernel.pars = [5, .1];
    else
        kernel = kernel.pars;
    end
    % function handle
    kernel.fhandle = @(pars, t) exp(-t/pars) * (1-exp(-1/pars)) ...
        / (1-exp(-nMax/pars));
elseif strcmpi(kernel_type, 'vector')
    % single vector
    kernel.type = 'vector';
    % parameter
    if ~exist('pars', 'var') || isempty(pars)
        kernel.pars = exp(-(1:nMax)/10);
    else
        kernel.pars = pars;
    end
    % function handle
    kernel.fhandle = @(pars, t) pars/sum(pars);
else
    % differencing of two exponential function:
    % ~  exp(-t/tau_d)-exp(-t/tau_r)
    kernel.type = 'exp2';

    % parameters
    if ~exist('pars', 'var') || isempty(pars)
        kernel.pars = [10, 1];
    else
        kernel.pars = pars;
    end
    % function handle
    kernel.fhandle = @(pars, t) (exp(-t/pars(1)) - exp(-t/pars(2)))  ...
        /( (1-exp(-nMax/pars(1)))/(1-exp(-1/pars(1))) ...
        - (1-exp(-nMax/pars(2)))/(1-exp(-1/pars(2))));
end

% lower and upper bounds for parameters
if ~exist('lb', 'var') || isempty(lb)
    kernel.lb = 0.5*kernel.pars;
else
    kernel.lb = lb;
end
if ~exist('ub', 'var') || isempty(ub)
    kernel.ub = 2*kernel.pars;
else
    kernel.ub = ub;
end

% bound the parameters of not
if ~exist('bound_pars', 'var')||isempty(bound_pars)
    kernel.bound_pars = false;
else
    kernel.bound_pars = bound_pars;
end


function tests = LineFeatureTest
    tests = functiontests(localfunctions)
    clc
end

function setupOnce(tc)
    im = testpattern('squares', 256, 256, 128);
    im = irotate(im, -0.3);
    edges = icanny(im);

    tc.TestData.edges = edges;
end

function teardownOnce(tc)
    close all
end

function constructor_test(tc)

    % create Hough object
    h = Hough(tc.TestData.edges);
    tc.verifyClass(h, 'Hough');

    s = char(h);
    tc.verifyClass(s, 'char');

    h.display;
    h.show();
end

function simple_test(tc)
    % check accumulators are correct size
    im = [0 0 0; 0 1 0; 0 0 0];
    h = Hough(im, 'nbins', [6 5]);
    tc.verifyEqual( size(h.A), [5 6]);

    h = Hough(im, 'nbins', 4);
    tc.verifyEqual( size(h.A), [5 4]);

    % simple example with 2 edge points
    edge = [
        0 0 0
        0 1 1
        0 0 0];
    out = [
        0     0     0     0
        2     0     0     0
        0     2     0     0
        0     0     2     1
        0     0     0     1
     ];
    h = Hough(edge, 'nbins', 4)
    tc.verifyEqual(h.A, out);

     % test vote weighting
     h = Hough(edge*2, 'nbins', 4);
     tc.verifyEqual( h.A, out*2);

     h = Hough(edge*2, 'nbins', 4, 'equal');
     tc.verifyEqual( h.A, out);

     % test xy input mode
     h = Hough([2 2; 2 3]', 'nbins', 4, 'points', [3 3]);
     tc.verifyEqual( h.A, out);

     % test xy input mode with weights
     h = Hough([2 2 5; 2 3 5]', 'nbins', 4, 'points', [3 3]);
     tc.verifyEqual( h.A, out*5);

     % test point, rather than image, mode
%      h=Hough([2 3; 2 2], 'points', 'nbins', 4)
%      tc.verifyEqual( h.A, out);
end

function square_test(tc)
    edges = tc.TestData.edges;

    h = Hough(edges);

    % create LineFeature objects
    lines = h.lines();
    tc.verifyClass(lines, 'LineFeature');

    tc.verifyEqual(numel(lines), 11);
    tc.verifyTrue( isempty(lines.length) );
    tc.verifyEqual( length(lines.theta), 11);
    tc.verifyEqual( length(lines.rho), 11);
    tc.verifyEqual( length(lines.strength), 11);

    s = char(lines);
    tc.verifyClass(s, 'char');
    tc.verifyEqual( size(s,1), 11);

end

function show_test(tc)
    h = Hough(tc.TestData.edges);

    clf
    h.show();
    a = gca;
    tc.verifyNotEmpty(a.Children);
    tc.verifyNumElements(a.Children, 1);
    tc.verifyMatches(a.Children.Type, 'image')

end

function plot_test(tc)
    h = Hough(tc.TestData.edges);
    clf
    h.plot('b');

    a = gca;
    tc.verifyNotEmpty(a.Children);
    tc.verifyNumElements(a.Children, 11);
    tc.verifyTrue(all( strcmp({a.Children.Type}, 'line') ) )
end

function square_suppress_test(tc)
    edges = tc.TestData.edges;

    % create new Hough and lines with non-local maxima suppression
    h = Hough(edges, 'suppress', 5);
    tc.verifyClass(h, 'Hough');

    lines = h.lines();
    tc.verifyClass(lines, 'LineFeature');
    tc.verifyEqual(numel(lines), 4);

    s = char(lines);
    tc.verifyClass(s, 'char');
    tc.verifyEqual(size(s,1), 4);

    % test line support
    lines = lines.seglength(edges);
    tc.verifyClass(lines, 'LineFeature');
    tc.verifyEqual(numel(lines), 4);

    tc.verifyEqual( numel(lines), 4);
    tc.verifyEqual( length(lines.theta), 4);
    tc.verifyEqual( length(lines.rho), 4);
    tc.verifyEqual( length(lines.length), 4);
    tc.verifyEqual( length(lines.strength), 4);
end


classdef TirePolynomial < VehicleDynamicsLateral.Tire
    % TirePolynomial Polynomial tire model
    %
    % It inherits methods from Tire.

    methods
        % Constructor
        function self = TirePolynomial()
            self.k1 = 115000;
            self.k2 = 560000;
        end

        function p = PlotTire(self)
            % Returns the handle of the curve
            alpha = (0:0.1:15)*pi/180;
            Fy = - self.Characteristic(alpha);
            p = plot(alpha*180/pi,Fy);
            grid on; box on;
            xlabel('Slip angle [deg]')
            ylabel('Lateral force [N]')
        end

        function Fy = Characteristic(self, alpha, varargin)
            % Lateral force
            Fy = - (self.k1 * alpha - self.k2 * alpha.^3);
        end
    end

    properties
        k1 % 1st polynomial coefficient, cornering stiffness [N/rad]
        k2 % 2nd polynomial coefficient [N/rad^3]
    end
end

%% See Also
%
% <../../index.html Home>
%


function r = gamrand(a, b, varargin);
%GAMRAND Random matrices from gamma distribution.
%
%   R = GAMRAND(A,B) returns a matrix of random numbers chosen
%   from the gamma distribution with parameters A and B.
%   The size of R is the common size of A and B if both are matrices.
%   If either parameter is a scalar, the size of R is the size of the other
%   parameter. Alternatively, R = GAMRAND(A,B,M,N) returns an M by N matrix.
%
%   Note: Parameterization as in (Neal, 1996).
%      A is mean of the distribution
%      B is degrees of freedom
%
%	See also INVGAMRAND
%
% Copyright (c) 1999 Aki Vehtari

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

error('No mex-file for this architecture. See Matlab help and convert.m in ./linuxCsource or ./winCsource for help.')


function [ indx_extract, n, indx ] = r8vec_indexed_heap_d_extract ( n, a, indx )

%*****************************************************************************80
%
%% R8VEC_INDEXED_HEAP_D_EXTRACT: extract from heap descending indexed R8VEC.
%
%  Discussion:
%
%    An R8VEC is a vector of R8's.
%
%    An indexed R8VEC is an R8VEC of data values, and an R8VEC of N indices,
%    each referencing an entry of the data vector.
%
%    The routine finds the maximum value in the heap, returns that value to the
%    user, deletes that value from the heap, and restores the heap to its
%    proper form.
%
%    Note that the argument N must be a variable, which will be decremented
%    before return, and that INDX will hold one less value on output than it
%    held on input.
%
%    This is one of three functions needed to model a priority queue.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Thomas Cormen, Charles Leiserson, Ronald Rivest,
%    Introduction to Algorithms,
%    MIT Press, 2001,
%    ISBN: 0262032937,
%    LC: QA76.C662.
%
%  Parameters:
%
%    Input, integer N, the number of items in the index vector.
%
%    Input, real A(*), the data vector.
%
%    Input, integer INDX(N), the index vector.
%
%    Output, integer INDX_EXTRACT, the index in A of the item of
%    maximum value, which has now been removed from the heap.
%
%    Output, integer N, the number of items in the revised index vector.
%
%    Output, integer INDX(N), the revised index vector.
%
  if ( n < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8VEC_INDEXED_HEAP_D_EXTRACT - Fatal error!\n' );
    fprintf ( 1, '  The heap is empty.\n' );
    error ( 'R8VEC_INDEXED_HEAP_D_EXTRACT - Fatal error!' )
  end
%
%  Get the index of the maximum value.
%
  indx_extract = indx(1);

  if ( n == 1 )
    n = 0;
    indx = [];
    return
  end
%
%  Shift the last index down.
%
  indx(1) = indx(n);
%
%  Restore the heap structure.
%
  n = n - 1;
  indx = r8vec_indexed_heap_d ( n, a, indx );

  return
end


function [dag,best_score] = learn_struct_hc(data, nodesizes, seeddag, varargin)
%
% LEARN_STRUCT_HC(data,seeddag) learns a structure of Bayesian net by Hill Climbing.
% dag = learn_struct_hc(data, nodesizes, seeddag)
%
% dag: the final structurre matrix
% Data : training data, data(i,m) is the m obsevation of node i
% Nodesizes: the size array of different nodes
% seeddag: given seed Dag for hill climbing, optional
%
% by Gang Li @ Deakin University (gli73@hotmail.com)

[N ncases] = size(data);
if (nargin < 3 )
    seeddag = zeros(N,N); % mk_rnd_dag(N); %call BNT function
elseif ~acyclic(seeddag)
    seeddag = mk_rnd_dag(N); %zeros(N,N);
end;

% set default params
scoring_fn = 'bic';
verbose  = 'yes';

% get params
args = varargin;
nargs = length(args);
if length(args) > 0
    if isstr(args{1})
    	for i = 1:2:nargs
    		switch args{i}
    		case 'scoring_fn', scoring_fn = args{i+1};
    		case 'verbose',  verbose  = strcmp(args{i+1},'yes');
    		end;
    	end;
    end;
end;

done = 0;
best_score = score_dags(data,nodesizes, {seeddag},'scoring_fn',scoring_fn);
while ~done
    [dags,op,nodes] = mk_nbrs_of_dag(seeddag);
    nbrs = length(dags);
    scores = score_dags(data, nodesizes, dags,'scoring_fn',scoring_fn);
    max_score = max(scores);
    new = find(scores == max_score );
    if ~isempty(new) & (max_score > best_score)
        p = sample_discrete(normalise(ones(1, length(new))));
        best_score = max_score;
        seeddag = dags{new(p)};
    else
        done = 1;
    end;
end;

dag = seeddag;


function [ypred]=knn(xapp,yapp,valY,X,k)

%
% knn implementation
%
% USE : [ypred]=knn(xapp,yapp,valY,X,k)
%
% Vincent Guigue 08/01/03

% check nargin

if nargin<4
  error('too few argumemnts');
elseif nargin<5
  k=3;
else
  if mod(k,2)==0
    error('k must be odd');
  end
end

if size(xapp,2)~=size(X,2)
  error('dimension incompatibility');
end

ndim = size(xapp,2);
nptxapp = size(xapp,1);
nptX = size(X,1);

% distance de X a xapp :
mat1 =  repmat(xapp, nptX,1);
%mat21 = reshape(X',1,nptX*ndim)
mat22 = repmat(X,1,nptxapp)';
mat2 = reshape(mat22 ,ndim, nptxapp*nptX)';
distance = mat1 - mat2 ;

distance = sum(distance.^2,2);
distance = reshape(distance,nptxapp,nptX);
[val kppv] = sort(distance,1);

% bilan sur les k premieres lignes
kppv = reshape(kppv(1:k,:),k*nptX,1);
Ykppv = yapp(kppv,1);
Ykppv = reshape(Ykppv,k,nptX);

% trouver le plus de reponses identique par colonne

vote = [];
for i=1:nptX
  for j=1:length(valY)
    vote(j,i)=size(find(Ykppv(:,i)==valY(j)),1);
  end
end

[val ind]=max(vote,[],1);
ypred = valY(ind);


function [M,P] = ghkf_predict(M,P,f,Q,f_param,p)
% GHKF_PREDICT - Gauss-Hermite Kalman filter prediction step
%
% Syntax:
%   [M,P] = GHKF_PREDICT(M,P,[f,Q,f_param,p])
%
% In:
%   M - Nx1 mean state estimate of previous step
%   P - NxN state covariance of previous step
%   f - Dynamic model function as a matrix A defining
%       linear function f(x) = A*x, inline function,
%       function handle or name of function in
%       form f(x,param)                   (optional, default eye())
%   Q - Process noise of discrete model   (optional, default zero)
%   f_param - Parameters of f               (optional, default empty)
%   p - Degree of approximation (number of quadrature points)
%
% Out:
%   M - Updated state mean
%   P - Updated state covariance
%
% Description:
%   Perform additive form Gauss-Hermite Kalman Filter prediction step.
%
%   Function f(.) should be such that it can be given a
%   DxN matrix of N sigma Dx1 points and it returns
%   the corresponding predictions for each sigma
%   point.
%
% See also:
%   GHKF_UPDATE, GHRTS_SMOOTH, GH_TRANSFORM

% History:
%   Aug 5,  2010 - Renamed from 'gh_predict' to 'ghkf_predict' (asolin)

% Copyright (C) 2009 Hartikainen, Särkkä, Solin
%
% $Id: gh_predict.m,v 1.2 2009/07/01 06:34:40 ssarkka Exp $
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.
%%

  %
  % Check which arguments are there
  %
  if nargin < 2
    error('Too few arguments');
  end
  if nargin < 3
    f = [];
  end
  if nargin < 4
    Q = [];
  end
  if nargin < 5
    f_param = [];
  end
  if nargin < 6
     p = [];
  end

  %
  % Apply defaults
  %
  if isempty(f)
    f = eye(size(M,1));
  end
  if isempty(Q)
    Q = zeros(size(M,1));
  end
  if isempty(p)
    p = 10;
  end

  %
  % Do transform and add process noise
  %
  tr_param = {p};
  [M,P] = gh_transform(M,P,f,f_param,tr_param);
  P = P + Q;


function [reg_min,G,reg_param,minG] = gcv(U,s,b,method)
%GCV Plot the GCV function and find its minimum.
%
% [reg_min,G,reg_param] = gcv(U,s,b,method)
% [reg_min,G,reg_param] = gcv(U,sm,b,method)  ,  sm = [sigma,mu]
%
% Plots the GCV-function
%          || A*x - b ||^2
%    G = -------------------
%        (trace(I - A*A_I)^2
% as a function of the regularization parameter reg_param.
% Here, A_I is a matrix which produces the regularized solution.
%
% The following methods are allowed:
%    method = 'Tikh' : Tikhonov regularization   (solid line )
%    method = 'tsvd' : truncated SVD or GSVD     (o markers  )
%    method = 'dsvd' : damped SVD or GSVD        (dotted line)
% If method is not specified, 'Tikh' is default.
%
% If any output arguments are specified, then the minimum of G is
% identified and the corresponding reg. parameter reg_min is returned.

% Per Christian Hansen, IMM, Dec. 16, 2003.

% Reference: G. Wahba, "Spline Models for Observational Data",
% SIAM, 1990.

% Set defaults.
if (nargin==3), method='Tikh'; end  % Default method.
npoints = 200;                      % Number of points on the curve.
smin_ratio = 16*eps;                % Smallest regularization parameter.

% Initialization.
[m,n] = size(U); [p,ps] = size(s);
beta = U'*b; beta2 = norm(b)^2 - norm(beta)^2;
if (ps==2)
  s = s(p:-1:1,1)./s(p:-1:1,2); beta = beta(p:-1:1);
end
if (nargout > 0), find_min = 1; else find_min = 0; end

if (strncmp(method,'Tikh',4) | strncmp(method,'tikh',4))

  % Vector of regularization parameters.
  reg_param = zeros(npoints,1); G = reg_param; s2 = s.^2;
  reg_param(npoints) = max([s(p),s(1)*smin_ratio]);
  ratio = (s(1)/reg_param(npoints))^(1/(npoints-1));
  for i=npoints-1:-1:1, reg_param(i) = ratio*reg_param(i+1); end

  % Intrinsic residual.
  delta0 = 0;
  if (m > n && beta2 > 0), delta0 = beta2; end

  % Vector of GCV-function values.
  for i=1:npoints
    G(i) = gcvfun(reg_param(i),s2,beta(1:p),delta0,m-n);
  end

  if nargout == 0
      % Plot GCV function.
      loglog(reg_param,G,'-'), xlabel('\lambda'), ylabel('G(\lambda)')
      title('GCV function')
  end

  % Find minimum, if requested.
  if (find_min)
    [minG,minGi] = min(G); % Initial guess.
    reg_min = fminbnd('gcvfun',...
      reg_param(min(minGi+1,npoints)),reg_param(max(minGi-1,1)),...
      optimset('Display','off'),s2,beta(1:p),delta0,m-n); % Minimizer.
    minG = gcvfun(reg_min,s2,beta(1:p),delta0,m-n); % Minimum of GCV function.
    if nargout == 0
        ax = axis;
        HoldState = ishold; hold on;
        loglog(reg_min,minG,'*r',[reg_min,reg_min],[minG/1000,minG],':r')
        title(['GCV function, minimum at \lambda = ',num2str(reg_min)])
        axis(ax)
        if (~HoldState), hold off; end
    end
  end

elseif (strncmp(method,'tsvd',4) | strncmp(method,'tgsv',4))

  % Vector of GCV-function values.
  rho2(p-1) = abs(beta(p))^2;
  if (m > n & beta2 > 0), rho2(p-1) = rho2(p-1) + beta2; end
  for k=p-2:-1:1, rho2(k) = rho2(k+1) + abs(beta(k+1))^2; end
  G = zeros(p-1,1);
  for k=1:p-1
    G(k) = rho2(k)/(m - k + (n - p))^2;
  end
  reg_param = (1:p-1)';

  % Plot GCV function.
  semilogy(reg_param,G,'o'), xlabel('k'), ylabel('G(k)')
  title('GCV function')

  % Find minimum, if requested.
  if (find_min)
    [minG,reg_min] = min(G);
    ax = axis;
    HoldState = ishold; hold on;
    semilogy(reg_min,minG,'*r',[reg_min,reg_min],[minG/1000,minG],':r')
    title(['GCV function, minimum at k = ',num2str(reg_min)])
    axis(ax);
    if (~HoldState), hold off; end
  end

elseif (strncmp(method,'dsvd',4) | strncmp(method,'dgsv',4))

  % Vector of regularization parameters.
  reg_param = zeros(npoints,1); G = reg_param;
  reg_param(npoints) = max([s(p),s(1)*smin_ratio]);
  ratio = (s(1)/reg_param(npoints))^(1/(npoints-1));
  for i=npoints-1:-1:1, reg_param(i) = ratio*reg_param(i+1); end

  % Intrinsic residual.
  delta0 = 0;
  if (m > n & beta2 > 0), delta0 = beta2; end

  % Vector of GCV-function values.
  for i=1:npoints
    G(i) = gcvfun(reg_param(i),s,beta(1:p),delta0,m-n,1);
  end

  % Plot GCV function.
  loglog(reg_param,G,':'), xlabel('\lambda'), ylabel('G(\lambda)')
  title('GCV function')

  % Find minimum, if requested.
  if (find_min)
    [minG,minGi] = min(G); % Initial guess.
    reg_min = fminbnd('gcvfun',...
      reg_param(min(minGi+1,npoints)),reg_param(max(minGi-1,1)),...
      optimset('Display','off'),s,beta(1:p),delta0,m-n,1); % Minimizer.
    minG = gcvfun(reg_min,s,beta(1:p),delta0,m-n,1); % Minimum of GCV function.
    ax = axis;
    HoldState = ishold; hold on;
    loglog(reg_min,minG,'*r',[reg_min,reg_min],[minG/1000,minG],':r')
    title(['GCV function, minimum at \lambda = ',num2str(reg_min)])
    axis(ax)
    if (~HoldState), hold off; end
  end

elseif (strncmp(method,'mtsv',4) | strncmp(method,'ttls',4))

  error('The MTSVD and TTLS methods are not supported')

else
  error('Illegal method')
end


function [ a, seed ] = r8sp_random ( m, n, nz_num, row, col, seed )

%*****************************************************************************80
%
%% R8SP_RANDOM sets a random R8SP matrix.
%
%  Discussion:
%
%    The R8SP storage format stores the row, column and value of each nonzero
%    entry of a sparse matrix.
%
%    It is possible that a pair of indices (I,J) may occur more than
%    once.  Presumably, in this case, the intent is that the actual value
%    of A(I,J) is the sum of all such entries.  This is not a good thing
%    to do, but I seem to have come across this in MATLAB.
%
%    The R8SP format is used by CSPARSE ("sparse triplet"), DLAP/SLAP
%    ("nonsymmetric SLAP triad"), by MATLAB, and by SPARSEKIT ("COO" format).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of the matrix.
%
%    Input, integer NZ_NUM, the number of nonzero elements in the matrix.
%
%    Input, integer ROW(NZ_NUM), COL(NZ_NUM), the row and column indices
%    of the nonzero elements.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real A(NZ_NUM), the nonzero elements of the matrix.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  [ a, seed ] = r8vec_uniform_01 ( nz_num, seed );

  return
end


function T = CreateDatabase(TrainDatabasePath)
% Align a set of face images (the training set T1, T2, ... , TM )
%
% Description: This function reshapes all 2D images of the training database
% into 1D column vectors. Then, it puts these 1D column vectors in a row to
% construct 2D matrix 'T'. Each column of 'T' is a training image, which has been reshaped into a 1D vector.
% Also, P is the total number of MxN training images and C is the number of
% classes.
%
%
% Argument:     TrainDatabasePath      - Path of the training database
%
% Returns:      T                      - A 2D matrix, containing all 1D image vectors.
%                                        The length of 1D column vectors is MN and 'T' will be a MNxP 2D matrix.
%
% See also: STRCMP, STRCAT, RESHAPE

% Original version by Amir Hossein Omidvarnia, October 2007
%                     Email: aomidvar@ece.ut.ac.ir

%%%%%%%%%%%%%%%%%%%%%%%% File management
TrainFiles = dir(TrainDatabasePath);
Train_Number = 0;

for i = 1:size(TrainFiles,1)
    if not(strcmp(TrainFiles(i).name,'.')|strcmp(TrainFiles(i).name,'..')|strcmp(TrainFiles(i).name,'Thumbs.db'))
        Train_Number = Train_Number + 1; % Number of all images in the training database
    end
end

%%%%%%%%%%%%%%%%%%%%%%%% Construction of 2D matrix from 1D image vectors
T = [];
for i = 1 : Train_Number

    % I have chosen the name of each image in databases as a corresponding
    % number. However, it is not mandatory!
    str = int2str(i);
    str = strcat('\',str,'.jpg');
    str = strcat(TrainDatabasePath,str);

    img = imread(str);
    img = rgb2gray(img);

    [irow icol] = size(img);

    temp = reshape(img',irow*icol,1);   % Reshaping 2D images into 1D image vectors
    T = [T temp]; % 'T' grows after each turn
end

T = double(T);


%CREATECONCENTRICSPHERESTESTSET  Creates test set
%
%     [samples, responses] = cv.createConcentricSpheresTestSet(nsamples, nfeatures, nclasses)
%
% ## Input
% * __nsamples__ returned samples count.
% * __nfeatures__ returned features count.
% * __nclasses__ number of classes.
%
% ## Output
% * __samples__ returned samples array `nsamples-by-nfeatures`.
% * __sampClasses__ corresponding classes labels `nsamples-by-1`.
%
% See also: cv.randMVNormal, cv.randGaussMixture
%


function linpack_s_test03 ( )

%*****************************************************************************80
%
%% TEST03 tests SCHUD.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 June 2009
%
%  Author:
%
%    John Burkardt
%
  p = 20;
  ldr = p;
  nz = 1;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST03\n' );
  fprintf ( 1, '  For single precision, general storage,\n' );
  fprintf ( 1, '  SCHUD updates a Cholesky decomposition.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  In this example, we use SCHUD to solve a\n' );
  fprintf ( 1, '  least squares problem R * b = z.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The number of equations is P = %d\n', p );
%
%  Initialize.
%
  r(1:p,1:p) = 0.0;
  z(1:p,1:nz) = 0.0;
  for i = 1 : p
    x(i) = i;
  end
%
%  Use SCHUD to form R, Z and RHO by adding X and Y a row at a time.
%  X is a row of the least squares matrix and Y the right hand side.
%
  seed = 123456789;

  for i = 1 : p
    [ row, seed ] = r4mat_uniform_01 ( 1, p, seed );
    y(1) = row(1:p) * x(1:p)';
    rho(1) = 0.0;
    [ r, z, rho, c, s ] = schud ( r, ldr, p, row, z, p, nz, y, rho );
  end
%
%  Generate the least squares solution, b = inverse ( R ) * Z.
%
  for j = 1 : nz

    b(1:p,1) = z(1:p,j);
    job = 01;

    [ b, info ] = strsl ( r, ldr, p, b, job );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Solution vector # %d\n', j );
    fprintf ( 1, '  (Should be (1,2,3...,n))\n' );
    fprintf ( 1, '\n' );

    for i = 1 : p
      if ( i <= 5 | p-5 < i )
        fprintf ( 1, '  %6d  %14f\n', i, b(i,1) );
      end
      if ( i == 5 )
        fprintf ( 1, '  ......  ..............\n' );
      end
    end

  end

  return
end