function [sol,output] = sdf_nls(model,options)
%SDF_NLS Structured data fusion by nonlinear least squares.
%   [sol,output] = sdf_nls(model) solves the data fusion problem described
%   by the structure model and returns the solution as the structure sol.
%   The model describes a data fusion problem with three fields:
%
%      model.variables
%
%         A structure or cell array of initializations for the variables in
%         the data fusion problem. Each field or cell in model.variables
%         represents a variable. A variable may be an array (such as
%         scalars, vectors, matrices and tensors) or a (nested) cell array
%         of arrays.
%
%      model.factors
%
%         A structure or cell array of factors. Each field or cell in
%         model.factors represents a factor. A factor is described by a
%         cell array of subfactors. After each subfactor has been
%         generated, the factor is constructed as the cell2mat of its
%         subfactors. A subfactor is a cell array in which the first
%         element is a reference to a variable, and the following elements
%         represent a sequence of transformations of that variable. If
%         model.variables is a structure, then a reference to a variable is
%         a string corresponding to the field name of the desired variable.
%         If model.variables is a cell array, then a reference to a
%         variable is the index of the cell containing the desired
%         variable. Transformations are supplied by functions which
%         describe their (linearized) behaviour. For example, the
%         transformation @struct_inv computes the factor as the matrix
%         inverse of the selected variable. All transformations must have
%         the function signature struct_mytrans(z,task). Instead of
%         supplying a reference to a variable, it is also possible to
%         supply a constant factor in the form of an array.
%
%      model.factorizations
%
%         A structure of data sets to jointly factorize. Each field in
%         model.factorizations represents a factorization. A factorization
%         is described by a structure containing two fields. The first
%         field has the field name 'data' and contains the (dense, sparse
%         or incomplete) array which is to be factorized. If the array
%         contains many zeros or a NaN, it is internally converted to a
%         sparse or incomplete tensor with fmt. The second field's
%         field name designates the type of factorization to compute of the
%         data set. Currently, two types are supported:
%
%         1. Canonical polyadic decomposition: a field 'cpd' should contain
%            a cell array of references to factors in model.factors. The
%            nth reference in the cell array corresponds to the nth factor
%            matrix of the CPD.
%
%         2. Block term decomposition: a field 'btd' should contain a cell
%            array of terms. Each term is itself a cell array containing
%            references to factors in model.factors. The nth reference in a
%            term corresponds to the nth factor matrix of that term. The
%            (N+1)th reference, where N is the number of dimensions of the
%            data set, in a term corresponds to the core tensor of that
%            term.
%
%         Additionally, two types of regularization are available:
%
%         3. L2 regularization: a field 'regL2' should contain a cell array
%            of references to factors in model.factors. This appends a term
%            to the objective function of the form 0.5*norm(F(:)-D(:),2)^2,
%            where F(:) and D(:) are the serialized factors in regL2 and
%            the data field respectively. The data field may be omitted, in
%            which case it is an all-zero vector.
%
%         4. L1 regularization: a field 'regL1' should contain a cell array
%            of references to factors in model.factors. This appends a term
%            to the objective function which is a smooth approximation of
%            0.5*norm(F(:)-D(:),1), where F(:) and D(:) are the serialized
%            factors in regL1 and the data field respectively. The data
%            field may be omitted, in which case it is an all-zero vector.
%
%   The algorithm proceeds to compute the joint factorization of the data
%   sets provided by models.factorizations by minimizing the sum of square
%   magnitude residuals of these factorizations. The output sol contains
%   the optimized variables in sol.variables and the corresponding factors
%   in sol.factors.
%
%   The structure output returns additional information:
%
%      output.Name  - The name of the selected algorithm.
%      output.<...> - The output of the selected algorithm.
%
%   sdf_nls(model,options) may be used to set the following options:
%
%      options.Algorithm =   - The desired optimization method.
%      [@nls_gncgs| ...
%       {@nls_gndl}|@nls_lm]
%      options.PC = true     - Whether or not to use a preconditioner, if
%                              available, for computing the Gauss-Newton
%                              step.
%      options.RelWeights =  - By supplying relative weights, the weights
%      ones(1,F)               options.Weights are computed as follows:
%                              options.Weights(f) = options.RelWeights(f)/
%                              (sum(options.RelWeights)*numel(data_f)) for
%                              each of the F factorizations in the model.
%      options.Weights       - The weight of the F factorizations in the
%                              data fusion model. The SDF objective
%                              function is \sum_f 0.5*options.Weights(f)*
%                              frob(model_f-data_f)^2, where model_f and
%                              data_f are the fth factorization and
%                              corresponding tensor. By default, weights
%                              are provided by options.RelWeights, but
%                              options.Weights has precedence if supplied.
%      options.<...>         - Parameters passed to the selected method,
%                              e.g., options.TolFun, options.TolX.
%                              See also help [options.Algorithm].
%
%   See also sdf_minf.

%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
%            Marc Van Barel (Marc.VanBarel@cs.kuleuven.be)
%            Lieven De Lathauwer (Lieven.DeLathauwer@kuleuven-kulak.be)
%
%   References:
%   [1] L. Sorber, M. Van Barel, L. De Lathauwer, "Structured data fusion,"
%       ESAT-SISTA Internal Report 13-177, KU Leuven, 2013.

% Check the options structure.
if nargin < 2, options = struct; end
isfunc = @(f)isa(f,'function_handle');
xsfunc = @(f)isfunc(f)&&exist(func2str(f),'file');
if ~isfield(options,'Algorithm')
    funcs = {@nls_gndl,@nls_gncgs,@nls_lm};
    options.Algorithm = funcs{find(cellfun(xsfunc,funcs),1)};
end
if ~isfield(options,'MaxIter'), options.MaxIter = 5000; end
if ~isfield(options,'CGMaxIter'), options.CGMaxIter = 15; end
if ~isfield(options,'Display'), options.Display = 0; end
if ~isfield(options,'JHasFullRank'), options.JHasFullRank = false; end
if ~isfield(options,'PC'), options.PC = true; end
if ~isfield(options,'TolLargeScale'), options.TolLargeScale = 0.02; end

% Convert model to internal format.
if ~iscell(model.variables)
    cache.variables.names = fieldnames(model.variables);
    model.variables = reshape(struct2cell(model.variables),1,[]);
else
    model.variables = model.variables(:).';
end
if ~iscell(model.factors)
    cache.factors.names = fieldnames(model.factors);
    model.factors = reshape(struct2cell(model.factors),1,[]);
else
    model.factors = model.factors(:).';
end
if ~iscell(model.factorizations)
    cache.factorizations.names = fieldnames(model.factorizations);
    model.factorizations = reshape(struct2cell(model.factorizations),1,[]);
else
    model.factorizations = model.factorizations(:).';
end
for I = 1:length(model.factors)
    if ~iscell(model.factors{I})
        model.factors{I} = model.factors(I);
    end
    if any(cellfun(@(f)isa(f,'function_handle'),model.factors{I}))
        model.factors{I} = model.factors(I);
    else
        for J = find(~cellfun(@iscell,model.factors{I}(:).'))
            model.factors{I}{J} = model.factors{I}(J);
        end
    end
end

% Set functions for saving state, computing the objective function,
% gradient and fast matrix-vector products. New models need only implement
% these four functions.
for I = 1:length(model.factorizations)
    fn = fieldnames(model.factorizations{I});
    model.factorizations{I}.type = find(~strcmp('data',fn));
    model.factorizations{I}.type = fn{model.factorizations{I}.type};
    model.factorizations{I}.factors = ...
        model.factorizations{I}.(model.factorizations{I}.type);
    model.factorizations{I} = ...
        rmfield(model.factorizations{I},model.factorizations{I}.type);
    switch model.factorizations{I}.type
        case 'cpd'
            model.factorizations{I}.state = @state_cpd;
            model.factorizations{I}.objfun = @objfun_cpd;
            model.factorizations{I}.grad = @grad_cpd;
            model.factorizations{I}.JHJx = @JHJx_cpd;
        case 'btd'
            model.factorizations{I}.state = @state_btd;
            model.factorizations{I}.objfun = @objfun_btd;
            model.factorizations{I}.grad = @grad_btd;
            model.factorizations{I}.JHJx = @JHJx_btd;
        case 'regL2'
            model.factorizations{I}.state = @state_regL2;
            model.factorizations{I}.objfun = @objfun_regL2;
            model.factorizations{I}.grad = @grad_regL2;
            model.factorizations{I}.JHJx = @JHJx_regL2;
        case 'regL1'
            model.factorizations{I}.state = @state_regL1;
            model.factorizations{I}.objfun = @objfun_regL1;
            model.factorizations{I}.grad = @grad_regL1;
            model.factorizations{I}.JHJx = @JHJx_regL1;
        otherwise
            error('sdf_nls:model','Model %s is not supported', ...
                model.factorizations{I}.type);
    end
end

% Fill in constants and dereference pointers from factors to variables.
cache.factors.isconst = cell(1,length(model.factors));
for I = 1:length(model.factors)
    cache.factors.isconst{I} = ...
        cellfun(@(f)isnumeric(f{1})&&~isscalar(f{1}),model.factors{I});
    for J = 1:numel(model.factors{I})
        if cache.factors.isconst{I}(J)
            const = model.factors{I}{J}{1};
            for K = 2:length(model.factors{I}{J})
                const = model.factors{I}{J}{K}(const);
            end
            model.factors{I}{J} = {const};
        elseif ischar(model.factors{I}{J}{1})
            model.factors{I}{J}{1} = ...
                find(strcmp(model.factors{I}{J}{1},cache.variables.names));
        end
    end
end

% Dereference and flatten pointers from factorizations to factors.
for I = 1:length(model.factorizations)
	model.factorizations{I}.factors = ...
        deref(model.factorizations{I}.factors);
end
function array = deref(array)
    for i = 1:length(array)
        if iscell(array{i})
            array{i} = deref(array{i});
        elseif ischar(array{i})
            array{i} = find(strcmp(array{i},cache.factors.names));
        end
    end
end

% Convert full data to incomplete/sparse format where appropriate.
cache.factorizations.isincomplete = false(1,length(model.factorizations));
cache.factorizations.issparse = false(1,length(model.factorizations));
for I = 1:length(model.factorizations)
    if ~isfield(model.factorizations{I},'data') || ...
       iscell(model.factorizations{I}.data)
        continue;
    end
    model.factorizations{I}.data = ...
        fmt(model.factorizations{I}.data,true);
    if isstruct(model.factorizations{I}.data)
        cache.factorizations.isincomplete(I) = ...
            model.factorizations{I}.data.incomplete;
        cache.factorizations.issparse(I) = ...
            model.factorizations{I}.data.sparse;
    end
end

% Cache expansion of variables into factors.
cache.factors.expanded = cell(size(model.factors));
cache.factors.sequence = cell(size(model.factors));
cache.factors.state = cell(size(model.factors));
for I = 1:length(model.factors)
    cache.factors.sequence{I} = cell(size(model.factors{I}));
    cache.factors.state{I} = cell(size(model.factors{I}));
    for J = 1:numel(model.factors{I})
        cache.factors.sequence{I}{J} = cell(1,length(model.factors{I}{J}));
        cache.factors.state{I}{J} = cell(1,length(model.factors{I}{J})-1);
    end
end
expand(model.variables,'cache');

% Cache factors' structure.
cache.factors.structure = ...
    cellfun(@structure,cache.factors.sequence,'UniformOutput',false);

% Cache offsets for variables and expanded factors.
cache.variables.offset = ...
    cumsum([1 cellfun(@(v)numel(serialize(v)),model.variables)]);
cache.factorizations.serialized = cell(size(model.factorizations));
cache.factorizations.offset = cell(size(model.factorizations));
cache.factorizations.suboffset = cell(size(model.factorizations));
for I = 1:length(model.factorizations)
    cache.factorizations.serialized{I} = ...
        serialize(model.factorizations{I}.factors).';
    cache.factorizations.offset{I} = ...
        cumsum([1 cellfun(@(v)numel(serialize(v)), ...
        cache.factors.expanded(cache.factorizations.serialized{I}))]);
    cum = cumsum([0 cellfun(@(f)sum(serialize( ...
        cellfun(@(s)numel(s{end}),f))),cache.factors.sequence( ...
        cache.factorizations.serialized{I}))]);
    fct = cellfun(@(f)cellfun(@(s)false(size(s{end})),f,'UniformOutput',...
        false),cache.factors.sequence( ...
        cache.factorizations.serialized{I}),'UniformOutput',false);
    cache.factorizations.suboffset{I} = cell(1,length(fct));
    for J = 1:length(fct)
        cache.factorizations.suboffset{I}{J} = cell(size(fct{J}));
        for K = 1:numel(fct{J})
            subfct = fct{J};
            subfct{K} = true(size(subfct{K}));
            subfct = find(cell2mat(subfct));
            if all(subfct(2:end) == subfct(1:end-1)+1)
                subfct = [subfct(1) subfct(end)];
            end
            cache.factorizations.suboffset{I}{J}{K} = subfct+cum(J);
        end
    end
end

% Set (relative) weights.
if isfield(options,'RelWeights') && isfield(options,'Weights')
    warning('sdf_nls:weights',['Both relative and absolute weights ' ...
        'are supplied, proceeding with absolute weights.']);
end
if ~isfield(options,'RelWeights')
    options.RelWeights = ones(1,length(model.factorizations));
end
options.RelWeights = options.RelWeights./sum(options.RelWeights);
if ~isfield(options,'Weights')
    for I = 1:length(model.factorizations)
        if isfield(model.factorizations{I},'data') && ...
           ~iscell(model.factorizations{I}.data)
            if isstruct(model.factorizations{I}.data)
                NUM = numel(model.factorizations{I}.data.val);
            else
                NUM = numel(model.factorizations{I}.data);
            end
        else
            NUM = cache.factorizations.offset{I}(end);
        end
        options.Weights(I) = 2*options.RelWeights(I)/NUM;
    end
end
if length(options.Weights) ~= length(model.factorizations)
    error('sdf_nls:weights',['The number of weights must equal the ' ...
        'number of factorizations.']);
end

% Initialize each factorization's state.
cache.factorizations.state = cell(size(model.factorizations));
for I = 1:length(model.factorizations)
    model.factorizations{I}.state(I,true);
end

% Run optimization algorithm.
dF.JHF = @grad;
dF.JHJx = @JHJx;
if options.PC && isfield(options,'M') && isa(options.M,'function_handle')
    dF.M = options.M;
end
[z,output] = options.Algorithm(@objfun,dF,model.variables,options);
output.Name = func2str(options.Algorithm);

% Return output.
if isfield(cache.variables,'names')
    sol.variables = cell2struct(z(:),cache.variables.names);
else
    sol.variables = z;
end
if isfield(cache.factors,'names')
    sol.factors = cell2struct(reshape(expand(z),[],1),cache.factors.names);
else
    sol.factors = expand(z);
end

function x = expand(z,where)
% Expands the variables into factors stored in the field factors.expanded,
% and stores their computational state in the field factors.state.

    % Save some references for speed.
    factors = model.factors;
    isconst = cache.factors.isconst;

    % Expand into cache or into the output variable x.
    if nargin == 2 && ischar(where)

        % For each ith factor...
        for i = 1:length(factors)
            % For each jth subfactor...
            for j = 1:numel(factors{i})
                if isconst{i}(j)
                    % Constant subfactor.
                    cache.factors.sequence{i}{j}{1} = factors{i}{j}{1};
                else
                    % For each kth transformation...
                    cache.factors.sequence{i}{j}{1} = z{factors{i}{j}{1}};
                    for k = 2:length(factors{i}{j})
                        [cache.factors.sequence{i}{j}{k}, ...
                            cache.factors.state{i}{j}{k-1}] = ...
                            factors{i}{j}{k}( ...
                            cache.factors.sequence{i}{j}{k-1},[]);
                    end
                end
            end
            if numel(cache.factors.sequence{i}) == 1
                cache.factors.expanded{i} = ...
                    cache.factors.sequence{i}{j}{end};
            else
                cache.factors.expanded{i} = ...
                    cell2mat(cellfun(@(f)f{end}, ...
                    cache.factors.sequence{i},'UniformOutput',false));
            end
        end

    else

        % For each ith factor...
        x = cell(size(factors));
        for i = 1:length(factors)
            % For each jth subfactor...
            sub = cell(size(factors{i}));
            for j = 1:numel(factors{i})
                if isconst{i}(j)
                    % Constant subfactor.
                    sub{j} = factors{i}{j}{1};
                else
                    % For each kth transformation...
                    sub{j} = z{factors{i}{j}{1}};
                    for k = 2:length(factors{i}{j})
                        sub{j} = factors{i}{j}{k}(sub{j},[]);
                    end
                end
            end
            if numel(sub) == 1, x{i} = sub{1};
            else x{i} = cell2mat(sub);
            end
        end

    end

end

function x = derivexpand(r)
% Linearly expand variables using their transformations' Jacobians.

    % Save some references for speed.
    factors = model.factors;
    voffset = cache.variables.offset;
    isconst = cache.factors.isconst;
    sequence = cache.factors.sequence;
    state = cache.factors.state;
    structure = cache.factors.structure;

    % For each ith factor...
    x = cell(size(factors));
    for i = 1:length(factors)
        % For each jth subfactor...
        sub = cell(size(factors{i}));
        for j = 1:numel(factors{i})
            if isconst{i}(j)
                % Constant subfactor.
                sub{j} = zeros(size(sequence{i}{j}{1}));
            else
                % For each kth transformation...
                seq = factors{i}{j};
                ftr = sequence{i}{j};
                stt = state{i}{j};
                dim = structure{i}{j}{1};
                sub{j} = r(voffset(seq{1}):voffset(seq{1}+1)-1);
                if isnumeric(dim)
                    if ~isempty(dim), sub{j} = reshape(sub{j},dim); end
                else sub{j} = deserialize(sub{j},dim);
                end
                for k = 2:length(seq)
                    task = stt{k-1};
                    task.l = [];
                    task.r = sub{j};
                    sub{j} = seq{k}(ftr{k-1},task);
                end
            end

        end
        if numel(sub) == 1, x{i} = sub{1};
        else x{i} = cell2mat(sub);
        end
    end

end

function y = derivcontract(f,y,l)
% Linearly contract factors using their transformations' Jacobians.

    % Save some references for speed.
    factors = model.factors;
    voffset = cache.variables.offset;
    isconst = cache.factors.isconst;
    sequence = cache.factors.sequence;
    state = cache.factors.state;
    structure = cache.factors.structure;
    serialized = cache.factorizations.serialized{f};
    soffset = cache.factorizations.suboffset{f};

    % Update y with Jacobian-contracted variables.
    for i = 1:length(serialized)
        % For each jth subfactor...
        idx = serialized(i);
        for j = 1:numel(factors{idx})

            % Skip this subfactor if it's constant.
            if isconst{idx}(j), continue; end

            % Apply sequence of Jacobian-vector products.
            seq = factors{idx}{j};
            ftr = sequence{idx}{j};
            stt = state{idx}{j};
            if size(soffset{i}{j},2) == 2
                sub = l(soffset{i}{j}(1):soffset{i}{j}(2));
            else
                sub = l(soffset{i}{j});
            end
            if length(seq) > 1
                dim = structure{idx}{j}{end};
                if isnumeric(dim)
                    if ~isempty(dim), sub = reshape(sub,dim); end
                else sub = deserialize(sub,dim);
                end
                for k = length(seq):-1:2
                    task = stt{k-1};
                    task.l = sub;
                    task.r = [];
                    sub = seq{k}(ftr{k-1},task);
                end
                sub = serialize(sub);
            end

            % Update y.
            jdx = voffset(seq{1}):voffset(seq{1}+1)-1;
            y(jdx) = y(jdx)+sub;

        end
    end

end

function z = getfactors(f,x)
% Retrieves factors of the fth factorization, given the factors x.

    % Deserialize the factors.
    z = deserialize_local(x,model.factorizations{f}.factors);
    function z = deserialize_local(x,dim)
        z = cell(size(dim));
        for i = 1:numel(dim)
            if iscell(dim{i})
                z{i} = deserialize_local(x,dim{i});
            else
                z{i} = x{dim{i}};
            end
        end
    end

end

function fval = objfun(z)

    % Expand the variables into factors.
    x = expand(z);

    % Compute objective function value.
    fval = 0;
    for i = find(options.Weights(:).' ~= 0)
        fval = fval+options.Weights(i)* ...
            model.factorizations{i}.objfun(i,getfactors(i,x));
    end

end

function grad = grad(z)

    % Expand the variables into factors.
    expand(z,'cache');

    % Let each factorization save intermediate computations in the cache.
    for i = 1:length(model.factorizations)
        model.factorizations{i}.state(i);
    end

    % Compute the gradient.
    grad = zeros(cache.variables.offset(end)-1,1);
    for i = find(options.Weights(:).' ~= 0)

        % Compute model's gradient.
        tmp = getfactors(i,cache.factors.expanded);
        JHF = model.factorizations{i}.grad(i,tmp);

        % Contract the factor matrices into variables.
        if i == 1
            grad = options.Weights(i)*derivcontract(i,grad,JHF);
        else
            grad = grad+options.Weights(i)*derivcontract(i,grad,JHF);
        end

    end

end

function y = JHJx(~,x)

    % Expand the variables into factors.
    x = derivexpand(x);

    % Compute (J(z)'*J(z))*x after expansion.
    y = zeros(cache.variables.offset(end)-1,1);
    for i = find(options.Weights(:).' ~= 0)

        % Apply fast matrix-vector product.
        tmpa = getfactors(i,cache.factors.expanded);
        tmpb = getfactors(i,x);
        JHJx = model.factorizations{i}.JHJx(i,tmpa,tmpb);

        % Contract the factor matrices into variables.
        if i == 1
            y = options.Weights(i)*derivcontract(i,y,JHJx);
        else
            y = y+options.Weights(i)*derivcontract(i,y,JHJx);
        end

    end

end

function [z,offset] = deserialize(z,dim,offset)
    if iscell(dim)
        v = z;
        z = cell(size(dim));
        if nargin < 3, offset = 0; end
        for i = 1:numel(z)
            if iscell(dim{i})
                [z{i},offset] = deserialize(v,dim{i},offset);
            else
                n = prod(dim{i}(:));
                z{i} = reshape(v(offset+(1:n)),dim{i});
                offset = offset+n;
            end
        end
    elseif ~isempty(dim)
        z = reshape(z,dim);
    end
end

function z = serialize(z)
    if iscell(z)
        for i = find(cellfun(@iscell,z(:).'))
            z{i} = serialize(z{i});
        end
        s = cellfun(@numel,z(:)); o = [0; cumsum(s)];
        c = z; z = zeros(o(end),1);
        for i = 1:length(s), z(o(i)+(1:s(i))) = c{i}(:); end
    else
        z = z(:);
    end
end

function dim = structure(z)
    if iscell(z)
        dim = cellfun(@size,z,'UniformOutput',false);
        for i = find(cellfun(@iscell,z(:).'))
            dim{i} = structure(z{i});
        end
    else
        dim = size(z);
        if numel(z) == dim(1), dim = []; end
    end
end

% Model: canonical polyadic decomposition ---------------------------------

function state_cpd(f,firstrun)
% Can read from model, cache and options, can save state by writing to the
% structure cache.factorizations.state{f}.

    if nargin == 2 && firstrun

        % Store the fraction of known elements.
        if cache.factorizations.isincomplete(f)
            cache.factorizations.scale{f} = ...
                length(model.factorizations{f}.data.val)./...
                prod(model.factorizations{f}.data.size);
        end

        % Set Block-Jacobi preconditioner if
        % - the model is a single CPD and
        % - each factor consists of exactly one subfactor and
        % - no subfactor is transformed and
        % - every subfactor is a reference to a variable and
        % - all variable references are unique and
        % - all factor references are unique and
        % - an NLS algorithm is used.
        options.BJ = false;
        var = cellfun(@(v)v{1}{1},model.factors,'UniformOutput',false);
        ftr = cell2mat(model.factorizations{f}.factors);
        if options.PC && numel(model.factorizations) == 1 && ...
                all(cellfun(@(v)length(v) == 1,model.factors)) && ...
                all(cellfun(@(v)length(v{1}) == 1,model.factors)) && ...
                all(cellfun(@isscalar,var)) && ...
                length(unique(cell2mat(var))) == length(cell2mat(var))&&...
                length(unique(ftr)) == length(ftr) && ...
                ~isempty(strfind(func2str(options.Algorithm),'nls'))
            options.M = @pc_cpd;
            options.BJ = true;
        end

    end

    % Cache the factor matrices' Gramians.
    idx = cache.factorizations.serialized{f};
    N = length(idx);
    R = size(cache.factors.expanded{idx(1)},2);
    cache.factorizations.state{f}.UHU = zeros(N,R*R);
    for n = 1:N
        tmp = cache.factors.expanded{idx(n)};
        tmp = conj(tmp'*tmp);
        cache.factorizations.state{f}.UHU(n,:) = tmp(:);
    end

    % Optionally cache the inverses of the Gramians for the preconditioner.
    % In a faster language, this should be the Cholesky factor instead.
    if options.BJ
        cache.factorizations.state{f}.invW = cell(1,N);
        for n = 1:N
            tmp = cache.factorizations.state{f}.UHU([1:n-1 n+1:N],:);
            if N > 2, tmp = prod(tmp,1); end
            cache.factorizations.state{f}.invW{n} = ...
                inv(reshape(tmp,[R R]));
        end
    end

end

function fval = objfun_cpd(f,z)

    % CPD objective function.
    T = model.factorizations{f}.data;
    isincomplete = cache.factorizations.isincomplete(f);
    issparse = cache.factorizations.issparse(f);
    if ~isincomplete || length(T.ind)/prod(T.size) > options.TolLargeScale
        fval = z{1}*kr(z(end:-1:2)).';
        if isincomplete, fval = fval(T.ind)-T.val;
        elseif issparse
            if ~isempty(T.ind), fval(T.ind) = fval(T.ind)-T.val; end
        else fval = fval-reshape(T,size(fval));
        end
    else
        fval = -T.val;
        for r = 1:size(z{1},2)
            tmp = z{1}(T.sub{1},r);
            for n = 2:length(z), tmp = tmp.*z{n}(T.sub{n},r); end
            fval = fval+tmp;
        end
    end
    if isincomplete
        T.val = fval;
        if ~isempty(T.matrix)
            T.matrix = sparse(double(T.sub{1}), ...
                double(1+idivide(T.ind-1,int64(size(T.matrix,1)))), ...
                double(fval),size(T.matrix,1),size(T.matrix,2));
        end
        cache.factorizations.residual{f} = T;
    else
        if issparse, size_tens = T.size;
        else size_tens = size(T); end
        cache.factorizations.residual{f} = reshape(fval,size_tens);
    end
    fval = 0.5*(fval(:)'*fval(:));

end

function grad = grad_cpd(f,z)

    % CPD scaled conjugate cogradient.
    E = cache.factorizations.residual{f};
    offset = cache.factorizations.offset{f};
    grad = zeros(offset(end)-1,1);
    for n = 1:length(z)
        tmp = full(mtkrprod(E,z,n));
        grad(offset(n):offset(n+1)-1) = tmp(:);
    end

end

function y = JHJx_cpd(f,z,x)

    % CPD fast Jacobian's Gramian vector product.
    % Ignores the fact that the tensor might be incomplete.
    R = size(z{1},2);
    N = length(z);
    offset = cache.factorizations.offset{f};
    UHU = cache.factorizations.state{f}.UHU;
    XHU = zeros(R,R,N);
    y = zeros(offset(end)-1,1);
    for n = 1:N
        Wn = UHU([1:n-1 n+1:N],:);
        if N > 2, Wn = prod(Wn,1); end
        XHU(:,:,n) = conj(x{n}'*z{n});
        y(offset(n):offset(n+1)-1) = x{n}*reshape(Wn,[R R]);
    end
    for n = 1:N-1
        idxn = offset(n):offset(n+1)-1;
        Wn = zeros(R);
        for m = n+1:N
            idxm = offset(m):offset(m+1)-1;
            if N == 2
                Wn = Wn+XHU(:,:,m);
                JHJmnx = z{m}*XHU(:,:,n);
            else
                Wnm = UHU([1:n-1 n+1:m-1 m+1:N],:);
                if N > 3, Wnm = prod(Wnm,1); end
                Wnm = reshape(Wnm,[R R]);
                Wn = Wn+Wnm.*XHU(:,:,m);
                JHJmnx = z{m}*(Wnm.*XHU(:,:,n));
            end
            y(idxm) = y(idxm)+JHJmnx(:);
        end
        JHJnx = z{n}*Wn;
        y(idxn) = y(idxn)+JHJnx(:);
    end

    % If incomplete, approximate the effect of missing entries.
    if cache.factorizations.isincomplete(f)
        y = y*cache.factorizations.scale{f};
    end

end

function x = pc_cpd(~,b)

    % Solve M*x = b, where M is a block-diagonal approximation for JHJ.
    % Equivalent to simultaneous ALS updates for each of the factors.
    x = zeros(size(b));
    offset = cache.factorizations.offset{1};
    invW = cache.factorizations.state{1}.invW;
    for n = 1:length(offset)-1
        idx = offset(n):offset(n+1)-1;
        tmp = reshape(b(idx),[],size(invW{1},1))*invW{n};
        x(idx) = tmp(:);
    end
    x = x/options.Weights;

    % If incomplete, approximate the effect of missing entries.
    if cache.factorizations.isincomplete(1)
        x = x/cache.factorizations.scale{1};
    end

end

% Model: block term decomposition -----------------------------------------

function state_btd(f,firstrun)
% Can read from model, cache and options, can save state by writing to the
% structure cache.factorizations.state{f}.

    if nargin == 2 && firstrun

        % Store the fraction of known elements.
        if cache.factorizations.isincomplete(f)
            cache.factorizations.scale{f} = ...
                length(model.factorizations{f}.data.val)./...
                prod(model.factorizations{f}.data.size);
        end

        % Set Block-Jacobi preconditioner if
        % - the model is a single BTD and
        % - each factor consists of exactly one subfactor and
        % - no subfactor is transformed and
        % - every subfactor is a reference to a variable and
        % - all variable references are unique and
        % - all factor references are unique and
        % - an NLS algorithm is used.
        options.BJ = false;
        var = cellfun(@(v)v{1}{1},model.factors,'UniformOutput',false);
        ftr = cache.factorizations.serialized{1};
        if options.PC && numel(model.factorizations) == 1 && ...
                all(cellfun(@(v)length(v) == 1,model.factors)) && ...
                all(cellfun(@(v)length(v{1}) == 1,model.factors)) && ...
                all(cellfun(@isscalar,var)) && ...
                length(unique(cell2mat(var))) == length(cell2mat(var))&&...
                length(unique(ftr)) == length(ftr) && ...
                ~isempty(strfind(func2str(options.Algorithm),'nls'))
            options.M = @pc_btd;
            options.BJ = true;
        end

    end

    % Cache the factor matrices' Gramians.
    U = getfactors(f,cache.factors.expanded);
    R = length(U);
    N = length(U{1})-1;
    [idx,jdx,kdx] = ndgrid(1:R,1:N,1:R);
    cache.factorizations.state{f}.UHU = ...
        arrayfun(@(i,n,j)U{i}{n}'*U{j}{n}, ...
        idx,jdx,kdx,'UniformOutput',false);

    % Optionally cache some results for the block-Jacobi preconditioner.
    if options.BJ
        [idx,jdx] = ndgrid(1:R,1:N);
        UHU = cache.factorizations.state{f}.UHU;
        cache.factorizations.state{f}.invSKS = arrayfun( ...
            @(r,n)inv(mtkronprod(U{r}{end},UHU(r,:,r),n)* ...
            conj(reshape(permute(U{r}{end},[1:n-1 n+1:N n]), ...
            [],size(U{r}{end},n)))),idx,jdx,'UniformOutput',false);
        cache.factorizations.state{f}.invUHU = arrayfun( ...
            @(r,n)inv(UHU{r,n,r}),idx,jdx,'UniformOutput',false);
    end

end

function fval = objfun_btd(f,z)

    % BTD objective function.
    T = model.factorizations{f}.data;
    isincomplete = cache.factorizations.isincomplete(f);
    issparse = cache.factorizations.issparse(f);
    if ~isincomplete || length(T.ind)/prod(T.size) > options.TolLargeScale
        fval = z{1}{1}*mtkronprod(z{1}{end},z{1}(1:end-1),1,'H');
        for r = 2:length(z)
            fval = fval+z{r}{1}*mtkronprod(z{r}{end},z{r}(1:end-1),1,'H');
        end
        if isincomplete, fval = fval(T.ind)-T.val;
        elseif issparse
            if ~isempty(T.ind), fval(T.ind) = fval(T.ind)-T.val; end
        else fval = fval-reshape(T,size(fval));
        end
    else
        fval = -T.val;
        for r = 1:length(z)
            size_core = cellfun('size',z{r}(1:end-1),2);
            idx = cell(1,length(size_core));
            S = z{r}{end};
            for i = 1:numel(S)
                [idx{:}] = ind2sub(size_core,i);
                tmp = S(idx{:})*z{r}{1}(T.sub{1},idx{1});
                for n = 2:length(size_core)
                    tmp = tmp.*z{r}{n}(T.sub{n},idx{n});
                end
                fval = fval+tmp;
            end
        end
    end
    if isincomplete
        T.val = fval;
        if ~isempty(T.matrix)
            T.matrix = sparse(double(T.sub{1}), ...
                double(1+idivide(T.ind-1,int64(size(T.matrix,1)))), ...
                double(fval),size(T.matrix,1),size(T.matrix,2));
        end
        cache.factorizations.residual{f} = T;
    else
        if issparse, size_tens = T.size;
        else size_tens = size(T); end
        cache.factorizations.residual{f} = reshape(fval,size_tens);
    end
    fval = 0.5*(fval(:)'*fval(:));

end

function grad = grad_btd(f,z)

    % BTD scaled conjugate cogradient.
    N = length(z{1})-1;
    E = cache.factorizations.residual{f};
    offset = cache.factorizations.offset{f};
    grad = zeros(offset(end)-1,1);
    cnt = 1;
    for r = 1:length(z)
        U = z{r}(1:N);
        S = conj(z{r}{end});
        for n = 1:N
            tmp = full(mtkronprod(E,U,n))* ...
                reshape(permute(S,[1:n-1 n+1:N n]),[],size(S,n));
            grad(offset(cnt):offset(cnt+1)-1) = tmp(:);
            cnt = cnt+1;
        end
        tmp = full(mtkronprod(E,U,0));
        grad(offset(cnt):offset(cnt+1)-1) = tmp;
        cnt = cnt+1;
    end

end

function y = JHJx_btd(f,z,x)

    % BTD fast Jacobian's Gramian vector product.
    % Ignores the fact that the tensor might be incomplete.
    R = length(z);
    N = length(z{1})-1;
    offset = cache.factorizations.offset{f};
    UHU = cache.factorizations.state{f}.UHU;
    [idx,jdx,kdx] = ndgrid(1:R,1:N,1:R);
    XHU = arrayfun(@(i,n,j)x{i}{n}'*z{j}{n}, ...
        idx,jdx,kdx,'UniformOutput',false);
    y = zeros(offset(end)-1,1);
    cnt = 1;
    for ri = 1:R

        % Factor matrices.
        for ni = 1:N
            idx = offset(cnt):offset(cnt+1)-1;
            Sri = permute(z{ri}{end},[1:ni-1 ni+1:N ni]);
            Sri = conj(reshape(Sri,[],size(Sri,N)));
            for rj = 1:R
                Srj = z{rj}{end};
                tmp = mtkronprod(x{rj}{end},UHU(rj,:,ri),ni);
                for nj = [1:ni-1 ni+1:N]
                    proj = UHU(rj,:,ri);
                    proj{nj} = XHU{rj,nj,ri};
                    tmp = tmp+mtkronprod(Srj,proj,ni);
                end
                tmp = z{rj}{ni}*(tmp*Sri);
                tmp = tmp+x{rj}{ni}* ...
                    (mtkronprod(z{rj}{end},UHU(rj,:,ri),ni)*Sri);
                if rj == 1, y(idx) = tmp(:);
                else y(idx) = y(idx)+tmp(:); end
            end
            cnt = cnt+1;
        end

        % Core tensor.
        idx = offset(cnt):offset(cnt+1)-1;
        for rj = 1:R
            Srj = z{rj}{end};
            tmp = mtkronprod(x{rj}{end},UHU(rj,:,ri),0);
            for nj = 1:N
                proj = UHU(rj,:,ri);
                proj{nj} = XHU{rj,nj,ri};
                tmp = tmp+mtkronprod(Srj,proj,0);
            end
            if rj == 1, y(idx) = tmp(:);
            else y(idx) = y(idx)+tmp(:); end
        end
        cnt = cnt+1;

    end

    % If incomplete, approximate the effect of missing entries.
    if cache.factorizations.isincomplete(f)
        y = y*cache.factorizations.scale{f};
    end

end

function x = pc_btd(~,b)

    % Solve M*x = b, where M is a block-diagonal approximation for JHJ.
    x = zeros(size(b));
    R = length(model.factorizations{1}.factors);
    N = length(model.factorizations{1}.factors{1})-1;
    offset = cache.factorizations.offset{1};
    invSKS = cache.factorizations.state{1}.invSKS;
    invUHU = cache.factorizations.state{1}.invUHU;
    cnt = 1;
    for r = 1:R
        for n = 1:N
            idx = offset(cnt):offset(cnt+1)-1;
            tmp = reshape(b(idx),[],size(invSKS{r,n},1))*invSKS{r,n};
            x(idx) = tmp(:);
            cnt = cnt+1;
        end
        idx = offset(cnt):offset(cnt+1)-1;
        size_core = cellfun('size',invUHU(r,:),1);
        x(idx) = mtkronprod(reshape(b(idx),size_core),invUHU(r,:),0);
        cnt = cnt+1;
    end
    x = x/options.Weights;

    % If incomplete, approximate the effect of missing entries.
    if cache.factorizations.isincomplete(1)
        x = x/cache.factorizations.scale{1};
    end

end

% Model: L2 regularization ------------------------------------------------

function state_regL2(f,firstrun)

    % Format right hand side, if available.
    if nargin == 2 && firstrun
         cache.factorizations.hasdata(f) = ...
             isfield(model.factorizations{f},'data');
         if cache.factorizations.hasdata(f) && ...
            ~iscell(model.factorizations{f}.data)
             model.factorizations{f}.data = ...
                 {model.factorizations{f}.data};
         end
         if cache.factorizations.hasdata(f)
             model.factorizations{f}.data = cellfun(@(d)full(d), ...
                 model.factorizations{f}.data,'UniformOutput',false);
         end
    end

end

function fval = objfun_regL2(f,z)
    % L2 regularization objective function.
    fval = 0;
    for i = 1:length(z)
        e = z{i}(:);
        if cache.factorizations.hasdata(f)
            e = e-model.factorizations{f}.data{i}(:);
        end
        fval = fval+(e'*e);
    end
    fval = 0.5*fval;
end

function grad = grad_regL2(f,z)
    % L2 regularization scaled conjugate cogradient.
    if cache.factorizations.hasdata(f)
        grad = cell2mat(cellfun(@(f,d)f(:)-d(:), ...
            z(:),model.factorizations{f}.data(:),'UniformOutput',false));
    else
        grad = cell2mat(cellfun(@(f)f(:),z(:),'UniformOutput',false));
    end
end

function y = JHJx_regL2(~,~,x)
    % L2 regularization Hessian vector product.
    y = cell2mat(cellfun(@(f)f(:),x(:),'UniformOutput',false));
end

% Model: L1 regularization ------------------------------------------------

function state_regL1(f,firstrun)

    % Format right hand side, if available.
    if nargin == 2 && firstrun
         cache.factorizations.hasdata(f) = ...
             isfield(model.factorizations{f},'data');
         if cache.factorizations.hasdata(f) && ...
            ~iscell(model.factorizations{f}.data)
             model.factorizations{f}.data = ...
                 {model.factorizations{f}.data};
         end
         if cache.factorizations.hasdata(f)
             model.factorizations{f}.data = cellfun(@(d)full(d), ...
                 model.factorizations{f}.data,'UniformOutput',false);
         end
         if ~isfield(options,'mu')
             if cache.factorizations.hasdata(f)
                m = max(abs(serialize(model.factorizations{f}.data)));
             else
                m = 0;
             end
             options.Mu = max(m,1)/100;
         end
    end

end

function fval = objfun_regL1(f,z)
    % Approximate L1 regularization objective function.
    fval = 0;
    for i = 1:length(z)
        e = z{i}(:);
        if cache.factorizations.hasdata(f)
            e = e-model.factorizations{f}.data{i}(:);
        end
        far = abs(e) > options.Mu;
        e(far) = abs(e(far));
        e2 = e(~far);
        e2 = e2.*conj(e2);
        e(~far) = 2*options.Mu*e2./(e2+options.Mu^2);
        fval = fval+sum(e);
    end
    fval = 0.5*fval;
end

function grad = grad_regL1(f,z)
    % Approximate L1 regularization scaled conjugate cogradient.
    if cache.factorizations.hasdata(f)
        grad = cell2mat(cellfun(@(f,d)f(:)-d(:), ...
            z(:),model.factorizations{f}.data(:),'UniformOutput',false));
    else
        grad = cell2mat(cellfun(@(f)f(:),z(:),'UniformOutput',false));
    end
    far = abs(grad) > options.Mu;
    grad(far) = grad(far)./(2*abs(grad(far)));
    grad(~far) = 2*options.Mu^3*grad(~far)./ ...
        (grad(~far).*conj(grad(~far))+options.Mu^2).^2;
end

function y = JHJx_regL1(f,z,x)
    % Approximate L1 regularization Hessian vector product.
    if cache.factorizations.hasdata(f)
        hess = cell2mat(cellfun(@(f,d)f(:)-d(:), ...
            z(:),model.factorizations{f}.data(:),'UniformOutput',false));
    else
        hess = cell2mat(cellfun(@(f)f(:),z(:),'UniformOutput',false));
    end
    if ~isreal(hess)
        error('sdf_nls:regL1',['Please use sdf_minf when applying L1 ' ...
            'regularization on complex factors.']);
    end
    far = abs(hess) > options.Mu;
    hess(far) = 0;
    if any(hess)
        hess(~far) = 2*options.Mu^3*(options.Mu^2-3*hess(~far).^2)./ ...
            (options.Mu^2+hess(~far).^2).^3;
        y = hess.*cell2mat(cellfun(@(f)f(:),x(:),'UniformOutput',false));
    else
        y = hess;
    end
end

end


% Ryan Steindl based on Robotics Toolbox for MATLAB (v6 and v9)
%
% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
function qp = mtimes(q1, q2)
%Quaternion.mtimes Multiply a quaternion object
%
% Q1*Q2   is a quaternion formed by Hamilton product of two quaternions.
% Q*V     is the vector V rotated by the quaternion Q
% Q*S     is the element-wise multiplication of quaternion elements by by the scalar S

    if isa(q1, 'Quaternion') & isa(q2, 'Quaternion')
    %QQMUL  Multiply unit-quaternion by unit-quaternion
    %
    %   QQ = qqmul(Q1, Q2)
    %
    %   Return a product of unit-quaternions.
    %
    %   See also: TR2Q

        % decompose into scalar and vector components
        s1 = q1.s;  v1 = q1.v;
        s2 = q2.s;  v2 = q2.v;

        % form the product
        qp = Quaternion([s1*s2-v1*v2' s1*v2+s2*v1+cross(v1,v2)]);

    elseif isa(q1, 'Quaternion') & isa(q2, 'double')

    %QVMUL  Multiply vector by unit-quaternion
    %
    %   VT = qvmul(Q, V)
    %
    %   Rotate the vector V by the unit-quaternion Q.
    %
    %   See also: QQMUL, QINV

        if length(q2) == 3
			qp = q1 * Quaternion([0 q2(:)']) * inv(q1);
            qp = qp.v(:);
        elseif length(q2) == 1
            qp = Quaternion(double(q1)*q2);
        else
            error('quaternion-vector product: must be a 3-vector or scalar');
        end

    elseif isa(q2, 'Quaternion') & isa(q1, 'double')
        if length(q1) == 3
            qp = q2 * Quaternion([0 q1(:)']) * inv(q2);
            qp = qp.v;
        elseif length(q1) == 1
            qp = Quaternion(double(q2)*q1);
        else
            error('quaternion-vector product: must be a 3-vector or scalar');
        end
    end
end


function Animate_Double_Pendulum(t,K,P)

%FUNCTION:
%   This function is used to generate an animation of the double pendulum,
%   using precomputed values.
%
%INPUTS:
%   t = a vector of time, (tout, returned by ode45)
%   K = Kinematics struct, returned by Double_Pendulum_Kinematics.m
%   P = Parameters struct, returned by Set_Parameters.m
%
%OUTPUTS:
%   Figure 1  --  Animation of double pendulum
%

%Which figure to plopt on?
figure(1);

%Get the various position vectors
P1_x = K.r_P1_O(1,:);
P1_y = K.r_P1_O(2,:);
P2_x = K.r_P2_O(1,:);
P2_y = K.r_P2_O(2,:);

%Get the font sizes
    TitleFontSize = P.plot.TitleFontSize;
    LabelFontSize = P.plot.LabelFontSize;

%Display a few things:
Parameters_to_Display = {...
        ['Slow Motion Factor: ' num2str(P.sim.slow_motion_factor)];...
        '';...
        ['Mass 1:  ' num2str(P.dyn.m1,4) '  kg'];...
        ['Mass 2:  ' num2str(P.dyn.m2,4) '  kg'];...
        ['Gravity:  ' num2str(P.dyn.g,4) '  m/s^2'];...
        ['Length:  ' num2str(P.dyn.L,4) '  m'];...
};

tic;    %Start a timer
Loop_Time = 0;    %store how long has the simulation been running
i=2;   %Start at second index (for interpolation purposes)
Max_i = length(t);
T_end = t(end);   %Ending time of one step
L = P.dyn.L;   %Leg length
Bounds = 1.1 * 2*L * [-1,1,-1,1];

while Loop_Time < T_end;  %Loop while the CPU time is less than the end of the simulation's time
    %The next few lines pull the time step that is closest to the real time
    Loop_Time = toc/P.sim.slow_motion_factor;   %Get the current time  (Taking slow motion into accunt if desired)
    while (i<Max_i) && (Loop_Time > (t(i))) %While we are not at the end of the data and the CPU time is ahead of the simulation time
        i=i+1;   %Go to the next time frame
    end
    %Now t(i-1) < Loop_Time < t(i) should be true

    %Linear interpolation scheme
        t_m = t(i-1);
        t_p = t(i);

        Pend_X_m = [0;P1_x(i-1); P2_x(i-1)];
        Pend_Y_m = [0;P1_y(i-1); P2_y(i-1)];

        Pend_X_p = [0;P1_x(i); P2_x(i)];
        Pend_Y_p = [0;P1_y(i); P2_y(i)];

        dPend_X = Pend_X_p - Pend_X_m;
        dPend_Y = Pend_Y_p - Pend_Y_m;

        dt = t_p - t_m;
        dt_cpu = Loop_Time - t_m;
        m = dt_cpu/dt;

        Pend_X = m*dPend_X + Pend_X_p;
        Pend_Y = m*dPend_Y + Pend_Y_p;

    clf
    plot(Pend_X,Pend_Y,'k-','LineWidth',6)
    hold on
    plot(Pend_X,Pend_Y,'b.','MarkerSize',50)
    plot(0,0,'ko','MarkerSize',40,'LineWidth',3)
    title(['Simulation Time: ' num2str(t(i)) ' seconds'],'FontSize',TitleFontSize)

    text(Bounds(2),mean(Bounds(3:4)),Parameters_to_Display,'FontSize',LabelFontSize);

    axis(Bounds); axis equal, axis manual; axis off;
    drawnow

end

end


function [sMap, sTrain] = som_seqtrain(sMap, D, varargin)

%SOM_SEQTRAIN  Use sequential algorithm to train the Self-Organizing Map.
%
% [sM,sT] = som_seqtrain(sM, D, [[argID,] value, ...])
%
%  sM     = som_seqtrain(sM,D);
%  sM     = som_seqtrain(sM,sD,'alpha_type','power','tracking',3);
%  [M,sT] = som_seqtrain(M,D,'ep','trainlen',10,'inv','hexa');
%
%  Input and output arguments ([]'s are optional):
%   sM      (struct) map struct, the trained and updated map is returned
%           (matrix) codebook matrix of a self-organizing map
%                    size munits x dim or  msize(1) x ... x msize(k) x dim
%                    The trained map codebook is returned.
%   D       (struct) training data; data struct
%           (matrix) training data, size dlen x dim
%   [argID, (string) See below. The values which are unambiguous can
%    value] (varies) be given without the preceeding argID.
%
%   sT      (struct) learning parameters used during the training
%
% Here are the valid argument IDs and corresponding values. The values which
% are unambiguous (marked with '*') can be given without the preceeding argID.
%   'mask'        (vector) BMU search mask, size dim x 1
%   'msize'       (vector) map size
%   'radius'      (vector) neighborhood radiuses, length 1, 2 or trainlen
%   'radius_ini'  (scalar) initial training radius
%   'radius_fin'  (scalar) final training radius
%   'alpha'       (vector) learning rates, length trainlen
%   'alpha_ini'   (scalar) initial learning rate
%   'tracking'    (scalar) tracking level, 0-3
%   'trainlen'    (scalar) training length
%   'trainlen_type' *(string) is the given trainlen 'samples' or 'epochs'
%   'train'      *(struct) train struct, parameters for training
%   'sTrain','som_train '  = 'train'
%   'alpha_type' *(string) learning rate function, 'inv', 'linear' or 'power'
%   'sample_order'*(string) order of samples: 'random' or 'ordered'
%   'neigh'      *(string) neighborhood function, 'gaussian', 'cutgauss',
%                          'ep' or 'bubble'
%   'topol'      *(struct) topology struct
%   'som_topol','sTopo l'  = 'topol'
%   'lattice'    *(string) map lattice, 'hexa' or 'rect'
%   'shape'      *(string) map shape, 'sheet', 'cyl' or 'toroid'
%
% For more help, try 'type som_seqtrain' or check out online documentation.
% See also  SOM_MAKE, SOM_BATCHTRAIN, SOM_TRAIN_STRUCT.

%%%%%%%%%%%%% DETAILED DESCRIPTION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% som_seqtrain
%
% PURPOSE
%
% Trains a Self-Organizing Map using the sequential algorithm.
%
% SYNTAX
%
%  sM = som_seqtrain(sM,D);
%  sM = som_seqtrain(sM,sD);
%  sM = som_seqtrain(...,'argID',value,...);
%  sM = som_seqtrain(...,value,...);
%  [sM,sT] = som_seqtrain(M,D,...);
%
% DESCRIPTION
%
% Trains the given SOM (sM or M above) with the given training data
% (sD or D) using sequential SOM training algorithm. If no optional
% arguments (argID, value) are given, a default training is done, the
% parameters are obtained from SOM_TRAIN_STRUCT function. Using
% optional arguments the training parameters can be specified. Returns
% the trained and updated SOM and a train struct which contains
% information on the training.
%
% REFERENCES
%
% Kohonen, T., "Self-Organizing Map", 2nd ed., Springer-Verlag,
%    Berlin, 1995, pp. 78-82.
% Kohonen, T., "Clustering, Taxonomy, and Topological Maps of
%    Patterns", International Conference on Pattern Recognition
%    (ICPR), 1982, pp. 114-128.
% Kohonen, T., "Self-Organized formation of topologically correct
%    feature maps", Biological Cybernetics 43, 1982, pp. 59-69.
%
% REQUIRED INPUT ARGUMENTS
%
%  sM          The map to be trained.
%     (struct) map struct
%     (matrix) codebook matrix (field .data of map struct)
%              Size is either [munits dim], in which case the map grid
%              dimensions (msize) should be specified with optional arguments,
%              or [msize(1) ... msize(k) dim] in which case the map
%              grid dimensions are taken from the size of the matrix.
%              Lattice, by default, is 'rect' and shape 'sheet'.
%  D           Training data.
%     (struct) data struct
%     (matrix) data matrix, size [dlen dim]
%
% OPTIONAL INPUT ARGUMENTS
%
%  argID (string) Argument identifier string (see below).
%  value (varies) Value for the argument (see below).
%
%  The optional arguments can be given as 'argID',value -pairs. If an
%  argument is given value multiple times, the last one is
%  used. The valid IDs and corresponding values are listed below. The values
%  which are unambiguous (marked with '*') can be given without the
%  preceeding argID.
%
%   'mask'       (vector) BMU search mask, size dim x 1. Default is
%                         the one in sM (field '.mask') or a vector of
%                         ones if only a codebook matrix was given.
%   'msize'      (vector) map grid dimensions. Default is the one
%                         in sM (field sM.topol.msize) or
%                         'si = size(sM); msize = si(1:end-1);'
%                         if only a codebook matrix was given.
%   'radius'     (vector) neighborhood radius
%                         length = 1: radius_ini = radius
%                         length = 2: [radius_ini radius_fin] = radius
%                         length > 2: the vector given neighborhood
%                                     radius for each step separately
%                                     trainlen = length(radius)
%   'radius_ini' (scalar) initial training radius
%   'radius_fin' (scalar) final training radius
%   'alpha'      (vector) learning rate
%                         length = 1: alpha_ini = alpha
%                         length > 1: the vector gives learning rate
%                                     for each step separately
%                                     trainlen is set to length(alpha)
%                                     alpha_type is set to 'user defined'
%   'alpha_ini'  (scalar) initial learning rate
%   'tracking'   (scalar) tracking level: 0, 1 (default), 2 or 3
%                         0 - estimate time
%                         1 - track time and quantization error
%                         2 - plot quantization error
%                         3 - plot quantization error and two first
%                             components
%   'trainlen'   (scalar) training length (see also 'tlen_type')
%   'trainlen_type' *(string) is the trainlen argument given in 'epochs'
%                         or in 'samples'. Default is 'epochs'.
%   'sample_order'*(string) is the sample order 'random' (which is the
%                         the default) or 'ordered' in which case
%                         samples are taken in the order in which they
%                         appear in the data set
%   'train'     *(struct) train struct, parameters for training.
%                         Default parameters, unless specified,
%                         are acquired using SOM_TRAIN_STRUCT (this
%                         also applies for 'trainlen', 'alpha_type',
%                         'alpha_ini', 'radius_ini' and 'radius_fin').
%   'sTrain', 'som_train' (struct) = 'train'
%   'neigh'     *(string) The used neighborhood function. Default is
%                         the one in sM (field '.neigh') or 'gaussian'
%                         if only a codebook matrix was given. Other
%                         possible values is 'cutgauss', 'ep' and 'bubble'.
%   'topol'     *(struct) topology of the map. Default is the one
%                         in sM (field '.topol').
%   'sTopol', 'som_topol' (struct) = 'topol'
%   'alpha_type'*(string) learning rate function, 'inv', 'linear' or 'power'
%   'lattice'   *(string) map lattice. Default is the one in sM
%                         (field sM.topol.lattice) or 'rect'
%                         if only a codebook matrix was given.
%   'shape'     *(string) map shape. Default is the one in sM
%                         (field sM.topol.shape) or 'sheet'
%                         if only a codebook matrix was given.
%
% OUTPUT ARGUMENTS
%
%  sM          the trained map
%     (struct) if a map struct was given as input argument, a
%              map struct is also returned. The current training
%              is added to the training history (sM.trainhist).
%              The 'neigh' and 'mask' fields of the map struct
%              are updated to match those of the training.
%     (matrix) if a matrix was given as input argument, a matrix
%              is also returned with the same size as the input
%              argument.
%  sT (struct) train struct; information of the accomplished training
%
% EXAMPLES
%
% Simplest case:
%  sM = som_seqtrain(sM,D);
%  sM = som_seqtrain(sM,sD);
%
% To change the tracking level, 'tracking' argument is specified:
%  sM = som_seqtrain(sM,D,'tracking',3);
%
% The change training parameters, the optional arguments 'train',
% 'neigh','mask','trainlen','radius','radius_ini', 'radius_fin',
% 'alpha', 'alpha_type' and 'alpha_ini' are used.
%  sM = som_seqtrain(sM,D,'neigh','cutgauss','trainlen',10,'radius_fin',0);
%
% Another way to specify training parameters is to create a train struct:
%  sTrain = som_train_struct(sM,'dlen',size(D,1),'algorithm','seq');
%  sTrain = som_set(sTrain,'neigh','cutgauss');
%  sM = som_seqtrain(sM,D,sTrain);
%
% By default the neighborhood radius goes linearly from radius_ini to
% radius_fin. If you want to change this, you can use the 'radius' argument
% to specify the neighborhood radius for each step separately:
%  sM = som_seqtrain(sM,D,'radius',[5 3 1 1 1 1 0.5 0.5 0.5]);
%
% By default the learning rate (alpha) goes from the alpha_ini to 0
% along the function defined by alpha_type. If you want to change this,
% you can use the 'alpha' argument to specify the learning rate
% for each step separately:
%  alpha = 0.2*(1 - log([1:100]));
%  sM = som_seqtrain(sM,D,'alpha',alpha);
%
% You don't necessarily have to use the map struct, but you can operate
% directly with codebook matrices. However, in this case you have to
% specify the topology of the map in the optional arguments. The
% following commads are identical (M is originally a 200 x dim sized matrix):
%  M = som_seqtrain(M,D,'msize',[20 10],'lattice','hexa','shape','cyl');
%
%  M = som_seqtrain(M,D,'msize',[20 10],'hexa','cyl');
%
%  sT= som_set('som_topol','msize',[20 10],'lattice','hexa','shape','cyl');
%  M = som_seqtrain(M,D,sT);
%
%  M = reshape(M,[20 10 dim]);
%  M = som_seqtrain(M,D,'hexa','cyl');
%
% The som_seqtrain also returns a train struct with information on the
% accomplished training. This is the same one as is added to the end of the
% trainhist field of map struct, in case a map struct is given.
%  [M,sTrain] = som_seqtrain(M,D,'msize',[20 10]);
%
%  [sM,sTrain] = som_seqtrain(sM,D); % sM.trainhist{end}==sTrain
%
% SEE ALSO
%
%  som_make         Initialize and train a SOM using default parameters.
%  som_batchtrain   Train SOM with batch algorithm.
%  som_train_struct Determine default training parameters.

% Copyright (c) 1997-2000 by the SOM toolbox programming team.
% http://www.cis.hut.fi/projects/somtoolbox/

% Version 1.0beta juuso 220997
% Version 2.0beta juuso 101199

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Check arguments

error(nargchk(2, Inf, nargin));  % check the number of input arguments

% map
struct_mode = isstruct(sMap);
if struct_mode,
  sTopol = sMap.topol;
else
  orig_size = size(sMap);
  if ndims(sMap) > 2,
    si = size(sMap); dim = si(end); msize = si(1:end-1);
    M = reshape(sMap,[prod(msize) dim]);
  else
    msize = [orig_size(1) 1];
    dim = orig_size(2);
  end
  sMap   = som_map_struct(dim,'msize',msize);
  sTopol = sMap.topol;
end
[munits dim] = size(sMap.codebook);

% data
if isstruct(D),
  data_name = D.name;
  D = D.data;
else
  data_name = inputname(2);
end
D = D(find(sum(isnan(D),2) < dim),:); % remove empty vectors from the data
[dlen ddim] = size(D);                % check input dimension
if dim ~= ddim, error('Map and data input space dimensions disagree.'); end

% varargin
sTrain = som_set('som_train','algorithm','seq','neigh', ...
		 sMap.neigh,'mask',sMap.mask,'data_name',data_name);
radius     = [];
alpha      = [];
tracking   = 1;
sample_order_type = 'random';
tlen_type  = 'epochs';

i=1;
while i<=length(varargin),
  argok = 1;
  if ischar(varargin{i}),
    switch varargin{i},
     % argument IDs
     case 'msize', i=i+1; sTopol.msize = varargin{i};
     case 'lattice', i=i+1; sTopol.lattice = varargin{i};
     case 'shape', i=i+1; sTopol.shape = varargin{i};
     case 'mask', i=i+1; sTrain.mask = varargin{i};
     case 'neigh', i=i+1; sTrain.neigh = varargin{i};
     case 'trainlen', i=i+1; sTrain.trainlen = varargin{i};
     case 'trainlen_type', i=i+1; tlen_type = varargin{i};
     case 'tracking', i=i+1; tracking = varargin{i};
     case 'sample_order', i=i+1; sample_order_type = varargin{i};
     case 'radius_ini', i=i+1; sTrain.radius_ini = varargin{i};
     case 'radius_fin', i=i+1; sTrain.radius_fin = varargin{i};
     case 'radius',
      i=i+1;
      l = length(varargin{i});
      if l==1,
        sTrain.radius_ini = varargin{i};
      else
        sTrain.radius_ini = varargin{i}(1);
        sTrain.radius_fin = varargin{i}(end);
        if l>2, radius = varargin{i}; tlen_type = 'samples'; end
      end
     case 'alpha_type', i=i+1; sTrain.alpha_type = varargin{i};
     case 'alpha_ini', i=i+1; sTrain.alpha_ini = varargin{i};
     case 'alpha',
      i=i+1;
      sTrain.alpha_ini = varargin{i}(1);
      if length(varargin{i})>1,
        alpha = varargin{i}; tlen_type = 'samples';
        sTrain.alpha_type = 'user defined';
      end
     case {'sTrain','train','som_train'}, i=i+1; sTrain = varargin{i};
     case {'topol','sTopol','som_topol'},
      i=i+1;
      sTopol = varargin{i};
      if prod(sTopol.msize) ~= munits,
        error('Given map grid size does not match the codebook size.');
      end
      % unambiguous values
     case {'inv','linear','power'}, sTrain.alpha_type = varargin{i};
     case {'hexa','rect'}, sTopol.lattice = varargin{i};
     case {'sheet','cyl','toroid'}, sTopol.shape = varargin{i};
     case {'gaussian','cutgauss','ep','bubble'}, sTrain.neigh = varargin{i};
     case {'epochs','samples'}, tlen_type = varargin{i};
     case {'random', 'ordered'}, sample_order_type = varargin{i};
     otherwise argok=0;
    end
  elseif isstruct(varargin{i}) && isfield(varargin{i},'type'),
    switch varargin{i}(1).type,
     case 'som_topol',
      sTopol = varargin{i};
      if prod(sTopol.msize) ~= munits,
        error('Given map grid size does not match the codebook size.');
      end
     case 'som_train', sTrain = varargin{i};
     otherwise argok=0;
    end
  else
    argok = 0;
  end
  if ~argok,
    disp(['(som_seqtrain) Ignoring invalid argument #' num2str(i+2)]);
  end
  i = i+1;
end

% training length
if ~isempty(radius) || ~isempty(alpha),
  lr = length(radius);
  la = length(alpha);
  if lr>2 || la>1,
    tlen_type = 'samples';
    if     lr> 2 && la<=1, sTrain.trainlen = lr;
    elseif lr<=2 && la> 1, sTrain.trainlen = la;
    elseif lr==la,        sTrain.trainlen = la;
    else
      error('Mismatch between radius and learning rate vector lengths.')
    end
  end
end
if strcmp(tlen_type,'samples'), sTrain.trainlen = sTrain.trainlen/dlen; end

% check topology
if struct_mode,
  if ~strcmp(sTopol.lattice,sMap.topol.lattice) || ...
	~strcmp(sTopol.shape,sMap.topol.shape) || ...
	any(sTopol.msize ~= sMap.topol.msize),
    warning('Changing the original map topology.');
  end
end
sMap.topol = sTopol;

% complement the training struct
sTrain = som_train_struct(sTrain,sMap,'dlen',dlen);
if isempty(sTrain.mask), sTrain.mask = ones(dim,1); end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% initialize

M        = sMap.codebook;
mask     = sTrain.mask;
trainlen = sTrain.trainlen*dlen;

% neighborhood radius
if length(radius)>2,
  radius_type = 'user defined';
else
  radius = [sTrain.radius_ini sTrain.radius_fin];
  rini = radius(1);
  rstep = (radius(end)-radius(1))/(trainlen-1);
  radius_type = 'linear';
end

% learning rate
if length(alpha)>1,
  sTrain.alpha_type ='user defined';
  if ~(abs(length(alpha)-trainlen) < ...
          1e4*eps(min(abs(length(alpha)),abs(trainlen)))),
    error('Trainlen and length of neighborhood radius vector do not match.')
  end
  if any(isnan(alpha)),
    error('NaN is an illegal learning rate.')
  end
else
  if isempty(alpha), alpha = sTrain.alpha_ini; end
  if strcmp(sTrain.alpha_type,'inv'),
    % alpha(t) = a / (t+b), where a and b are chosen suitably
    % below, they are chosen so that alpha_fin = alpha_ini/100
    b = (trainlen - 1) / (100 - 1);
    a = b * alpha;
  end
end

% initialize random number generator
rand('state',sum(100*clock));

% distance between map units in the output space
%  Since in the case of gaussian and ep neighborhood functions, the
%  equations utilize squares of the unit distances and in bubble case
%  it doesn't matter which is used, the unitdistances and neighborhood
%  radiuses are squared.
Ud = som_unit_dists(sTopol).^2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Action

update_step = 100;
mu_x_1 = ones(munits,1);
samples = ones(update_step,1);
r = samples;
alfa = samples;

qe = 0;
start = clock;
if tracking >  0, % initialize tracking
  track_table = zeros(update_step,1);
  qe = zeros(floor(trainlen/update_step),1);
end

printedbytes = 0;

for t = 1:trainlen,

  % Every update_step, new values for sample indeces, neighborhood
  % radius and learning rate are calculated. This could be done
  % every step, but this way it is more efficient. Or this could
  % be done all at once outside the loop, but it would require much
  % more memory.
  ind = rem(t,update_step); if ind==0, ind = update_step; end
  if ind==1,
    steps = [t:min(trainlen,t+update_step-1)];
    % sample order
    switch sample_order_type,
     case 'ordered', samples = rem(steps,dlen)+1;
     case 'random',  samples = ceil(dlen*rand(update_step,1)+eps);
    end

    % neighborhood radius
    switch radius_type,
     case 'linear',       r = rini+(steps-1)*rstep;
     case 'user defined', r = radius(steps);
    end
    r=r.^2;        % squared radius (see notes about Ud above)
    r(r==0) = eps; % zero radius might cause div-by-zero error

    % learning rate
    switch sTrain.alpha_type,
     case 'linear',       alfa = (1-steps/trainlen)*alpha;
     case 'inv',          alfa = a ./ (b + steps-1);
     case 'power',        alfa = alpha * (0.005/alpha).^((steps-1)/trainlen);
     case 'user defined', alfa = alpha(steps);
    end
  end

  % find BMU
  x = D(samples(ind),:);                 % pick one sample vector
  known = ~isnan(x);                     % its known components
  Dx = M(:,known) - x(mu_x_1,known);     % each map unit minus the vector
  [qerr bmu] = min((Dx.^2)*mask(known)); % minimum distance(^2) and the BMU

  % tracking
  if tracking>0,
    track_table(ind) = sqrt(qerr);
    if ind==update_step,
      n = ceil(t/update_step);
      qe(n) = mean(track_table);
      printedbytes = trackplot(M,D,tracking,start,n,qe,printedbytes);
    end
  end

  % neighborhood & learning rate
  % notice that the elements Ud and radius have been squared!
  % (see notes about Ud above)
  switch sTrain.neigh,
  case 'bubble',   h = (Ud(:,bmu)<=r(ind));
  case 'gaussian', h = exp(-Ud(:,bmu)/(2*r(ind)));
  case 'cutgauss', h = exp(-Ud(:,bmu)/(2*r(ind))) .* (Ud(:,bmu)<=r(ind));
  case 'ep',       h = (1-Ud(:,bmu)/r(ind)) .* (Ud(:,bmu)<=r(ind));
  end
  h = h*alfa(ind);

  % update M
  M(:,known) = M(:,known) - h(:,ones(sum(known),1)).*Dx;

end; % for t = 1:trainlen

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Build / clean up the return arguments

if tracking, fprintf(1,'\n'); end

% update structures
sTrain = som_set(sTrain,'time',datestr(now,0));
if struct_mode,
  sMap = som_set(sMap,'codebook',M,'mask',sTrain.mask,'neigh',sTrain.neigh);
  tl = length(sMap.trainhist);
  sMap.trainhist(tl+1) = sTrain;
else
  sMap = reshape(M,orig_size);
end

return;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% subfunctions

%%%%%%%%
function [count] = trackplot(M,D,tracking,start,n,qe, printedbytes)

  l = length(qe);
  elap_t = etime(clock,start);
  tot_t = elap_t*l/n;
  % Carriage return does not work as it should (even on UNIX) when printing
  % to screen, so let's do this instead
  fprintf(1, repmat('\b', 1, printedbytes));
  count = fprintf(1,'Training: %3.0f/ %3.0f s',elap_t,tot_t);
  switch tracking
   case 1,
   case 2,
    plot(1:n,qe(1:n),(n+1):l,qe((n+1):l))
    title('Quantization errors for latest samples')
    drawnow
   otherwise,
    subplot(2,1,1), plot(1:n,qe(1:n),(n+1):l,qe((n+1):l))
    title('Quantization error for latest samples');
    subplot(2,1,2), plot(M(:,1),M(:,2),'ro',D(:,1),D(:,2),'b.');
    title('First two components of map units (o) and data vectors (+)');
    drawnow
  end
  % end of trackplot


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

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

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

load mnistvh
load mnisthp
load mnisthp2
load mnistpo

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

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

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

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

for epoch = 1:maxepoch

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

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

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

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

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

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

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

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

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

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

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

 end

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

end


function [strika,dipa,rakea,dipdia,strikb,dipb,rakeb,dipdib,ierr] = pt2pl(trendp,plungp,trendt,plungt)

    % compute strike dip and rake (and dip direction) of two nodal planes from trend and plung of P and T axes
    %
    %     usage:
    %     call pt2pl(trendp,plungp,trendt,plungt,strika,dipa,rakea
    %    1,dipdia,strikb,dipb,rakeb,dipdib,ierr)
    %
    %     arguments:
    %     trendp         trend of P axis in degrees (INPUT)
    %     plungp         plunge of P axis in degrees (INPUT)
    %     trendt         trend of P axis in degrees (INPUT)
    %     plungt         plunge of P axis in degrees (INPUT)
    %     strika         strike angle of first nodal plane in degrees (OUTPUT)
    %     dipa           dip angle of first nodal plane in degrees (OUTPUT)
    %     rakea          rake angle of first nodal plane in degrees (OUTPUT)
    %     dipdia         dip direction angle of first nodal plane in degrees (OUTPUT)
    %     strikb         strike angle of second nodal plane in degrees (OUTPUT)
    %     dipb           dip angle of second nodal plane in degrees (OUTPUT)
    %     rakeb          rake angle of second nodal plane in degrees (OUTPUT)
    %     dipdib         dip direction angle of second nodal plane in degrees (OUTPUT)
    %     ierr           error indicator (OUTPUT)
    %
    %     errors:
    %     1              input TREND angle of P axis out of range
    %     2              input PLUNGE angle P axis out of range
    %     3              1+2
    %     4              input TREND angle of P axis out of range
    %     5              input PLUNGE angle P axis out of range
    %     6              4+5
    %     8,9,10         internal errors
    %
    % c
    %      call fpsset
    amistr=-360.;
    amastr=360.;
    amidip=0.;
    amadip=90.;
    amirak=-360.;
    amarak=360.;
    amitre=-360.;
    amatre=360.;
    amiplu=0.;
    amaplu=90.;
    orttol=2.;
    ovrtol=0.001;
    tentol=0.0001;
    dtor=0.017453292519943296;
    c360=360.;
    c90=90.;
    c0=0.;
    c1=1.;
    c2=2.;
    c3=3.;
    % c
    %      call ax2ca(trendp,plungp,px,py,pz,ierr)
    [px,py,pz,ierr] = focal_ax2ca(trendp,plungp);
    if (ierr ~= 0)
        disp(['PT2PL: ierr=' num2str(ierr)]);
        return;
    end

    %      call ax2ca(trendt,plungt,tx,ty,tz,ierr)
    [tx,ty,tz,ierr] = focal_ax2ca(trendt,plungt);
    if (ierr ~= 0)
        ierr = ierr + 3;
        disp(['PT2PL: ierr=' num2str(ierr)]);
        return;
    end

    [anx,any,anz,dx,dy,dz,ierr] = focal_pt2nd(px,py,pz,tx,ty,tz);
    if (ierr ~= 0)
        ierr = 8;
        disp(['PT2PL: ierr=' num2str(ierr)]);
        return;
    end
    [strika,dipa,rakea,dipdia,ierr] = focal_nd2pl(anx,any,anz,dx,dy,dz);
    if (ierr ~= 0)
        ierr = 9;
        disp(['PT2PL: ierr=' num2str(ierr)]);
        return;
    end
    [strikb,dipb,rakeb,dipdib,ierr] = focal_nd2pl(dx,dy,dz,anx,any,anz);
    if (ierr ~= 0)
        ierr = 10;
        disp(['PT2PL: ierr=' num2str(ierr)]);
        return;
    end
end


function dM = Bloch(t, M, Param, Pulse)
%BLOCH Bloch-McConnell ordinary differential equations.
%   dM = Bloch(t, M, Param, Pulse)
%
%   --args--
%   t: Function handle variable, represents the time.
%   M : magnetization vector [Mxf, Myf, Mzf, Mzr]
%   Param: Tissue parameter structure.
%
%          -fields-
%          M0f: Free pool equilibrium magnetization.
%          M0r: Restricted pool equilibrium magnetization. (M0r = F*M0f)
%          R1f: Free pool longitudinal relaxation rate.
%          R1r: Restricted pool longitudinal relaxation rate.
%          R2f: Free pool transverse relaxation rate.
%          kf: Free pool magnetization exchange rate.
%          kr: Restricted pool magnetization exchange rate. (= kf/F)
%          G: Lineshape value of the restricted pool (generated by
%             computeG.m)
%
%   Pulse: RF Pulse structure (generated by GetPulse.m).
%
%   Reference: R. Mark Henkelman, Xuemei Huang, Qing-San Xiang, G. J.
%               Stanisz, Scott D. Swanson, Michael J. Bronskill.
%               Quantitative Interpretation of Magnetization Transfer,Mag.
%               Res. in Med., 29, 759-766, Eqs. 1-6, (1993)
%
%
%   See also COMPUTEG, GETPULSE, BLOCH, BLOCHNOMT.
%

dM = zeros(4,1);

if (nargin < 4)
    omega  = 0;
    omega2 = 0;
    delta  = 0;
else
    omega  = Pulse.omega(t);
    omega2 = Pulse.omega2(t);
    delta  = Pulse.delta;
end

W = pi*Param.G.*omega2;

if isfield(Param,'T2f')
    R2f = 1./Param.T2f;
else
    R2f = Param.R2f;
end

if isfield(Param,'F')
    kf = Param.kr*Param.F;
    M0r = Param.M0f*Param.F;
else
    kf = Param.kf;
    M0r = Param.M0r;
end

dM(1) = -R2f*M(1) - 2*pi*delta*M(2);
dM(2) = -R2f*M(2) + 2*pi*delta*M(1) + omega*M(3);
dM(3) =  Param.R1f*(Param.M0f-M(3)) - kf*M(3) + Param.kr*M(4) - omega*M(2);
dM(4) =  Param.R1r*(M0r-M(4)) + kf*M(3) - Param.kr*M(4) - W*M(4);

end


function eect2k = eect2000 (date1, date2)

%  equation of the equinoxes complementary terms, consistent with
%  iau 2000 resolutions.

%  annexe to iers conventions 2000, chapter 5

%  capitaine, n., wallace, p.t., & mccarthy, d.d. (2003). astron. &
%  astrophys. 406, pp. 1135-1149, table 3.
%  iers conventions (2010), chapter 5, p. 60, table 5.2e.
%  (table 5.2e presented in the printed publication is a truncated
%  series. the full series, which is used in novas, is available on
%  the iers conventions center website in file tab5.2e.txt.)
%  ftp://tai.bipm.org/iers/conv2010/chapter5/

%  input

%   date1, date2 = tt date (jd = date1 + date2)

%  output

%   eect2k = complementary terms (radians)

%  this revision:  2002 november 13
%                  references updated 2010 november 26

% ported from NOVAS 3.1

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

% 2 pi

d2pi = 6.283185307179586476925287d0;

% arc seconds to radians

das2r = 4.848136811095359935899141d-6;

% reference epoch (j2000), jd

dj0 = 2451545.0d0;

% days per julian century

djc = 36525.0d0;

%  -----------------------------------------
%  the series for the ee complementary terms
%  -----------------------------------------

%  number of terms in the series

ne0 = 33;

ne1 = 1;

%  argument coefficients for t^0

ke0 = [0,  0,  0,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    0,  0,  0,  0,  2,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    0,  0,  2, -2,  3,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    0,  0,  2, -2,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    0,  0,  2, -2,  2,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    0,  0,  2,  0,  3,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    0,  0,  2,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    0,  0,  0,  0,  3,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    0,  1,  0,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    0,  1,  0,  0, -1,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    1,  0,  0,  0, -1,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    1,  0,  0,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    0,  1,  2, -2,  3,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    0,  1,  2, -2,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    0,  0,  4, -4,  4,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    0,  0,  1, -1,  1,  0, -8, 12,  0,  0,  0,  0,  0,  0;
    0,  0,  2,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    0,  0,  2,  0,  2,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    1,  0,  2,  0,  3,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    1,  0,  2,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    0,  0,  2, -2,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    0,  1, -2,  2, -3,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    0,  1, -2,  2, -1,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    0,  0,  0,  0,  0,  0,  8,-13,  0,  0,  0,  0,  0, -1;
    0,  0,  0,  2,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    2,  0, -2,  0, -1,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    1,  0,  0, -2,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    0,  1,  2, -2,  2,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    1,  0,  0, -2, -1,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    0,  0,  4, -2,  4,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    0,  0,  2, -2,  4,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    1,  0, -2,  0, -3,  0,  0,  0,  0,  0,  0,  0,  0,  0;
    1,  0, -2,  0, -1,  0,  0,  0,  0,  0,  0,  0,  0,  0;];

% transpose

ke0 = ke0';

%  argument coefficients for t^1

ke1 = [0,  0,  0,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0];

% transpose

ke1 = ke1';

%  sine and cosine coefficients for t^0

se0 = [+2640.96d-6,          -0.39d-6;
    +63.52d-6,          -0.02d-6;
    +11.75d-6,          +0.01d-6;
    +11.21d-6,          +0.01d-6;
    -4.55d-6,          +0.00d-6;
    +2.02d-6,          +0.00d-6;
    +1.98d-6,          +0.00d-6;
    -1.72d-6,          +0.00d-6;
    -1.41d-6,          -0.01d-6;
    -1.26d-6,          -0.01d-6;
    -0.63d-6,          +0.00d-6;
    -0.63d-6,          +0.00d-6;
    +0.46d-6,          +0.00d-6;
    +0.45d-6,          +0.00d-6;
    +0.36d-6,          +0.00d-6;
    -0.24d-6,          -0.12d-6;
    +0.32d-6,          +0.00d-6;
    +0.28d-6,          +0.00d-6;
    +0.27d-6,          +0.00d-6;
    +0.26d-6,          +0.00d-6;
    -0.21d-6,          +0.00d-6;
    +0.19d-6,          +0.00d-6;
    +0.18d-6,          +0.00d-6;
    -0.10d-6,          +0.05d-6;
    +0.15d-6,          +0.00d-6;
    -0.14d-6,          +0.00d-6;
    +0.14d-6,          +0.00d-6;
    -0.14d-6,          +0.00d-6;
    +0.14d-6,          +0.00d-6;
    +0.13d-6,          +0.00d-6;
    -0.11d-6,          +0.00d-6;
    +0.11d-6,          +0.00d-6;
    +0.11d-6,          +0.00d-6;];

se0 = se0';

%  sine and cosine coefficients for t^1

se1 = [-0.87d-6, +0.00d-6];

% transpose

se1 = se1';

%  interval between fundamental epoch j2000.0 and current date (jc)

t = ((date1 - dj0) + date2) / djc;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  fundamental arguments (from iers conventions 2000)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%  mean anomaly of the moon

fa(1) = anmp ((485868.249036d0 + (715923.2178d0 + (31.8792d0 ...
    + (0.051635d0 + (-0.00024470d0) * t) * t) * t) * t) * das2r ...
    + mod (1325.0d0 * t, 1.0d0) * d2pi);

%  mean anomaly of the sun

fa(2) = anmp ((1287104.793048d0 + (1292581.0481d0 + (-0.5532d0 ...
    + (+0.000136d0 + (-0.00001149d0) * t) * t) * t) * t) * das2r ...
    + mod (99.0d0 * t, 1.0d0) * d2pi);

%  mean longitude of the moon minus mean longitude of the ascending
%  node of the moon

fa(3) = anmp (( 335779.526232d0 + (295262.8478d0 + (-12.7512d0 ...
    + (-0.001037d0 + (0.00000417d0) * t) * t) * t) * t) * das2r ...
    + mod (1342d0 * t, 1d0) * d2pi);

%  mean elongation of the moon from the sun

fa(4) = anmp ((1072260.703692d0 + (1105601.2090d0 + (-6.3706d0 ...
    + (0.006593d0 + (-0.00003169d0) * t) * t) * t) * t) * das2r ...
    + mod (1236d0 * t, 1d0) * d2pi);

%  mean longitude of the ascending node of the moon

fa(5) = anmp ((450160.398036d0 + (-482890.5431d0 + (7.4722d0 ...
    + (0.007702d0 + (-0.00005939d0) * t) * t) * t) * t) * das2r ...
    + mod (-5.0d0 * t, 1.0d0) * d2pi);

fa(6) = anmp (4.402608842d0 + 2608.7903141574d0 * t);
fa(7) = anmp (3.176146697d0 + 1021.3285546211d0 * t);
fa(8) = anmp (1.753470314d0 +  628.3075849991d0 * t);
fa(9) = anmp (6.203480913d0 +  334.0612426700d0 * t);
fa(10) = anmp (0.599546497d0 +   52.9690962641d0 * t);
fa(11) = anmp (0.874016757d0 +   21.3299104960d0 * t);
fa(12) = anmp (5.481293872d0 +    7.4781598567d0 * t);
fa(13) = anmp (5.311886287d0 +    3.8133035638d0 * t);
fa(14) =      (0.024381750d0 +    0.00000538691d0 * t) * t;

%  evaluate the ee complementary terms

s0 = 0.0d0;

s1 = 0.0d0;

for i = ne0: -1: 1

    a = 0.0d0;

    for j = 1:14

        a = a + ke0(j, i) * fa(j);

    end

    s0 = s0 + (se0(1, i) * sin(a) + se0(2, i) * cos(a));

end

for i = ne1: -1: 1

    a = 0.0d0;

    for j = 1:14

        a = a + ke1(j, i) * fa(j);

    end

    s1 = s1 + (se1(1, i) * sin(a) + se1(2, i) * cos(a));

end

eect2k = (s0 + s1 * t) * das2r;


function rayPlot(ray,ord)
%+========================================================================+
%|                                                                        |
%|           OPENRAY - LIBRARY FOR TRI-DIMENSIONAL RAY TRACING            |
%|           openRay is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : rayPlot.m                                     |
%|    #    |   VERSION    : 0.41                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 01.04.2018                                    |
%| ( === ) |   SYNOPSIS   : Plot trajectory and last direction            |
%|  `---'  |                                                              |
%+========================================================================+

% Graphical configuration
figure(gcf)
off = ~ishold;
if off
    hold on
end

% Plot trajectory
if (ord > -1)
    for i = 2:ord+1
        Pim1 = ray.pos{i-1};
        Pi   = ray.pos{i};
        plot3([Pim1(:,1) Pi(:,1)]',[Pim1(:,2) Pi(:,2)]',[Pim1(:,3) Pi(:,3)]','o-b')
    end
end

% Plot direction
if (ord == -1) || (length(ray.pos) == 1)
    P = ray.pos{end};
    U = ray.dir;
    quiver3(P(:,1),P(:,2),P(:,3),U(:,1),U(:,2),U(:,3),'r')
end

% Hold
if off
    hold off
end
end