%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Find the start time offset based on the cyclic prefix
%
% This method is immune to the issues that plague the ZC sequences (frequency offset causes a time shift in the
% correlation results)
%
% It's best to provide this function an upsampled copy of the burst to help fix any fractional time offset that might be
% present
%
% @param samples Complex IQ samples that make up the full burst
% @param sample_rate Sample rate (in Hz) of the provided samples
% @return start_offset Sample index that the burst starts at (first sample of the first cyclic prefix)
function [start_offset] = find_sto_cp(samples, sample_rate)
    [long_cp_len, short_cp_len] = get_cyclic_prefix_lengths(sample_rate);
    fft_size = get_fft_size(sample_rate);
    cyclic_prefix_length_schedule = [...
        long_cp_len, ...
        short_cp_len, ...
        short_cp_len, ...
        short_cp_len, ...
        short_cp_len, ...
        short_cp_len, ...
        short_cp_len, ...
        short_cp_len, ...
        long_cp_len];
    num_ofdm_symbols = length(cyclic_prefix_length_schedule);

    full_burst_len = sum(cyclic_prefix_length_schedule) + (fft_size * num_ofdm_symbols);
    num_tests = length(samples) - full_burst_len;
    scores_cp_sto = zeros(1, num_tests);

    for idx=1:num_tests
        offset = idx;
        scores = zeros(1, num_ofdm_symbols);

        % Extract and correlate the samples that each cyclic prefix is expected
        % to be at
        for cp_idx=1:num_ofdm_symbols
            cp_len = cyclic_prefix_length_schedule(cp_idx);

            % Extract the full OFDM symbol including cyclic prefix
            window = samples(offset:offset + fft_size + cp_len - 1);

            % Extract the cyclic prefix and the final samples of the symbol
            left = window(1:cp_len);
            right = window(end - cp_len + 1:end);

            % Correlate the two windows
            scores(cp_idx) = abs(xcorr(left, right, 0));

            % Move the sample pointer forward by the full symbol size
            offset = offset + cp_len + fft_size;
        end

        % In the real DroneID the first OFDM symbol needs to be ignored since
        % it isn't always present.  So, just average the correlation scores of
        % all but the first element
        scores_cp_sto(idx) = sum(scores(2:end)) / (length(scores) - 1);

    end

    % Find the index of the highest score
    [~, start_offset] = max(scores_cp_sto);
end


function varargout = transformPoint3d(pts, transfo, varargin)
%TRANSFORMPOINT3D Transform a point with a 3D affine transform.
%
%   PT2 = transformPoint3d(PT1, TRANS);
%   PT2 = transformPoint3d(X1, Y1, Z1, TRANS);
%   where PT1 has the form [xp yp zp], and TRANS is a 3-by-3, 3-by-4, or
%   4-by-4 matrix, returns the point transformed according to the affine
%   transform specified by TRANS.
%
%   The function accepts transforms given using the following formats:
%   [a b c]   ,   [a b c j] , or [a b c j]
%   [d e f]       [d e f k]      [d e f k]
%   [g h i]       [g h i l]      [g h i l]
%                                [0 0 0 1]
%
%   PT2 = transformPoint3d(PT1, TRANS)
%   also work when PT1 is a N-by-3-by-M-by-P-by-ETC array of double. In
%   this case, PT2 has the same size as PT1.
%
%   PT2 = transformPoint3d(X1, Y1, Z1, TRANS);
%   also work when X1, Y1 and Z1 are 3 arrays with the same size. In this
%   case, PT2 will be a 1-by-3 cell containing {X Y Z} outputs of size(X1).
%
%   [X2, Y2, Z2] = transformPoint3d(...);
%   returns the result in 3 different arrays the same size as the input.
%   This form can be useful when used with functions like meshgrid or warp.
%
%   MESH2 = transformPoint3d(MESH, TRANS)
%   transforms the field 'vertices' of the struct MESH and returns the same
%   struct with the transformed vertices.
%   (It is recommended to use the function 'transformMesh', within the
%   "meshes3d" module).
%
%   See also
%     points3d, transforms3d, transformMesh, createTranslation3d
%     createRotationOx, createRotationOy, createRotationOz, createScaling
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2005-02-10
% Copyright 2005-2022 INRA - TPV URPOI - BIA IMASTE

%% Parse input arguments

% Check special case: if first argument is a struct with a field named
% 'vertices', then the output will be the same struct, but with the
% transformed vertices.
if nargin == 2 && isstruct(pts) && isfield(pts, 'vertices')
    mesh = pts;
    mesh.vertices = transformPoint3d(mesh.vertices, transfo);
    varargout = {mesh};
    return;
end

% Parse x, y, and z coordinates of input points from input arguments
if nargin == 2
    % Point coordinates are given in a single N-by-3-by-M-by-etc argument.
    % Preallocate x, y, and z to size N-by-1-by-M-by-etc, then fill them in
    dim = size(pts);
    dim(2) = 1;
    [x, y, z] = deal(zeros(dim, class(pts)));
    x(:) = pts(:,1,:);
    y(:) = pts(:,2,:);
    z(:) = pts(:,3,:);

elseif nargin == 4
    % Point coordinates are given in 3 different arrays
    x = pts;
    y = transfo;
    z = varargin{1};
    transfo = varargin{2};
    dim = size(x);

else
    error('MatGeom:geom3d:WrongInputArgumentNumber', ...
        'Requires number of input arguments to be either 2 or 4');
end

%% Process transformation matrix

% extract the linear and the translation parts of the matrix
linear = transfo(1:3, 1:3)';
trans = [0 0 0];
if size(transfo, 2) > 3
    trans = transfo(1:3, 4)';
end

%% Main processing

% convert coordinates
try
    % vectorial processing, if there is enough memory.
    % same as:
    % res = (transfo * [x(:) y(:) z(:) ones(NP, 1)]')';
    res = bsxfun(@plus, [x(:) y(:) z(:)] * linear, trans);

    % Back-fill x,y,z with new result (saves calling costly reshape())
    x(:) = res(:,1);
    y(:) = res(:,2);
    z(:) = res(:,3);

catch ME
    disp(ME.message)
    % process each point one by one, writing in existing array
    NP = numel(x);
    for i = 1:NP
        res = [x(i) y(i) z(i)] * linear + trans;
        x(i) = res(1);
        y(i) = res(2);
        z(i) = res(3);
    end
end

% process output arguments
if nargout <= 1
    % results are stored in a unique array
    if length(dim) > 2 && dim(2) > 1
        warning('geom3d:shapeMismatch',...
            'Shape mismatch: Non-vector xyz input should have multiple x,y,z output arguments. Cell {x,y,z} returned instead.')
        varargout{1} = {x,y,z};
    else
        varargout{1} = [x y z];
    end

elseif nargout == 3
    % results are returned in three array with same size.
    varargout = {x, y, z};
end


function s = addrice(s)
%ADDRICE Add the Rician distribution.

%   Copyright 1993-2004 The MathWorks, Inc.
%   $Revision: 1.1.6.10 $  $Date: 2004/02/01 22:10:34 $

j = length(s) + 1;
s(j).name = 'Rician';
s(j).code = 'rician';
s(j).pnames = {'s' 'sigma'};
s(j).pdescription = {'noncentrality' 'scale'};
s(j).prequired = [false false];
s(j).fitfunc = @ricefit;
s(j).likefunc = @ricelike;
s(j).cdffunc = @ricecdf;
s(j).pdffunc = @ricepdf;
s(j).invfunc = @riceinv;
s(j).statfunc = @ricestat;
s(j).loginvfunc = [];
s(j).logcdffunc = [];
s(j).hasconfbounds = false;
s(j).censoring = true;
s(j).paramvec = true;
s(j).support = [0 Inf];
s(j).closedbound = [false false];
s(j).iscontinuous = true;
s(j).islocscale = false;
s(j).uselogpp = false;

% ==== Rician distribution functions ====

% these distribution functions do not yet handle arrays of parameters

function y = ricepdf(x,s,sigma)
%RICEPDF Rician probability density function (pdf).
s(s < 0) = NaN;
sigma(sigma <= 0) = NaN;

x(x<0) = 0;
sigsq = sigma.^2;
expon = (x.^2 + s.^2)./(2.*sigsq);
y = (x./sigsq) .* exp(-expon) .* besseli(0, x.*s./sigsq);
y(expon > (log(realmax(class(x)))-1)) = 0; % fix up 0*Inf

function p = ricecdf(x,s,sigma)
%RICECDF Rician cumulative distribution function (cdf).
s(s < 0) = NaN;
sigma(sigma <= 0) = NaN;

x(x<0) = 0;
p = ncx2cdf((x./sigma).^2, 2, (s./sigma).^2);

function x = riceinv(p,s,sigma)
%RICEINV Inverse of the Rician cumulative distribution function (cdf).
s(s < 0) = NaN;
sigma(sigma <= 0) = NaN;

x = sigma .* sqrt(ncx2inv(p, 2, (s./sigma).^2));

function r = ricernd(s,sigma,varargin)
%RICERND Random arrays from the Rician distribution.
s(s < 0) = NaN;
sigma(sigma <= 0) = NaN;

[err, sizeOut] = statsizechk(2,s,sigma,varargin{:});
if err > 0
    error('stats:ricernd:InconsistentSizes','Size information is inconsistent.');
end

r = sigma .* sqrt(ncx2rnd(2, (s./sigma).^2, sizeOut));

function [m,v] = ricestat(s,sigma)
%RICESTAT Mean and variance for the Rician distribution.
s(s < 0) = NaN;
sigma(sigma <= 0) = NaN;

t = .5 .* (s./sigma).^2;
m = sigma.*sqrt(.5.*pi).*exp(-.5.*t) .* ((1+t).*besseli(0,.5.*t) + t.*besseli(1,.5.*t));
v = 2.*sigma.^2 + s.^2 - m.^2;

function [nlogL,acov] = ricelike(params,data,cens,freq)
%RICELIKE Negative log-likelihood for the Rician distribution.
if nargin < 4 || isempty(freq), freq = ones(size(data)); end
if nargin < 3 || isempty(cens), cens = zeros(size(data)); end

nlogL = rice_nloglf(params, data, cens, freq);
if nargout > 1
    acov = mlecov(params, data, 'nloglf',@rice_nloglf, 'cens',cens, 'freq',freq);
end

% ==== Rician fitting functions ====

function [phat,pci] = ricefit(x,alpha,cens,freq,opts)
%NAKAFIT Parameter estimates and confidence intervals for Rician data.

if nargin < 2 || isempty(alpha), alpha = .05; end
if nargin < 3 || isempty(cens), cens = zeros(size(x)); end
if nargin < 4 || isempty(freq), freq = ones(size(x)); end
if nargin < 5, opts = []; end

if any(x <= 0)
    error('stats:ricefit:BadData','The data in X must be positive');
end

% Moment estimators of the uncensored data as starting point
% E[x.^2] = s.^2 + 2.*sigma.^2
% E[x.^4] = s.^4 + 8.*s.^2.*sigma.^2 + 8.*sigma.^4
xsqunc = x(cens == 0).^2;
meanxsq = mean(xsqunc); meanx4th = mean(xsqunc);
if meanxsq.^2 < meanx4th && meanx4th < 2.*meanxsq.^2
    s4th = 2.*meanxsq.^2 - meanx4th;
    ssq = sqrt(s4th);
    sigsq = .5.*(meanxsq - ssq);
    start = [sqrt(ssq) sqrt(sigsq)];
else
    start = cast([1 1],class(x));
end

% The default options include turning fminsearch's display off.  This
% function gives its own warning/error messages, and the caller can turn
% display on to get the text output from fminsearch if desired.
options = statset(statset('ricefit'), opts);
tolBnd = options.TolBnd;
options = optimset(options);

% Maximize the log-likelihood with respect to s and sigma.
[phat,nll,err,output] = ...
    fminsearch(@rice_nloglf, start, options, x, cens, freq, tolBnd);
if (err == 0)
    % fminsearch may print its own output text; in any case give something
    % more statistical here, controllable via warning IDs.
    if output.funcCount >= options.MaxFunEvals
        wmsg = 'Maximum likelihood estimation did not converge.  Function evaluation limit exceeded.';
    else
        wmsg = 'Maximum likelihood estimation did not converge.  Iteration limit exceeded.';
    end
    warning('stats:ricefit:IterOrEvalLimit',wmsg);
elseif (err < 0)
    error('stats:ricefit:NoSolution',...
          'Unable to reach a maximum likelihood solution.');
end

% Compute CIs using a normal approximation for phat.
if nargout > 1
    acov = mlecov(phat, x, 'nloglf',@rice_nloglf, 'cens',cens, 'freq',freq);
    probs = [alpha/2; 1-alpha/2];
    se = sqrt(diag(acov))';
    pci = norminv([probs probs], [phat; phat], [se; se]);
end

function nll = rice_nloglf(parms, x, cens, freq, tolBnd)
%RICE_NLOGLF Objective function for Rician maximum likelihood.
s = parms(1);
sigma = parms(2);
sigsq = sigma.^2;

% Restrict sigma to the open interval (0, Inf).
if nargin > 4
    if s < tolBnd || sigma < tolBnd
        nll = Inf;
        return
    end
end

bess0 = besseli(0, x.*s./sigsq);
rsq = (x.^2 + s.^2)./(2.*sigsq);
L = -rsq + log(bess0) + log(x./sigsq);
ncen = sum(freq.*cens);
if ncen > 0
    cen = (cens == 1);
    xcen = x(cen);
    L(cen) = log(marcumq(s./sigma,xcen./sigma));
end
nll = -sum(freq .* L);

% Don't have derivatives of the Marcum's Q, so can't compute an analytic
% gradient with censoring.
%
% if nargout > 1
%     dlogbess0 = besseli(1, x.*s./sigsq) ./ bess0;
%     dL1 = (-s + dlogbess0.*x) ./ sigsq;
%     dL2 = (rsq - 1 - dlogbess0.*x.*s./sigsq) ./ sigma;
%     if ncen > 0
% %         dL1(cen) = ;
% %         dL2(cen) = ;
%     end
%     ngrad = -[sum(freq .* dL1) sum(freq .* dL2)];
% end

function Q = marcumq(a,b)
% Q = MARCUMQ(A,B) returns Marcum's "Q" function.

if isa(a,'single') || isa(b,'single')
   Q = repmat(single(NaN), size(a));
else
   Q = repmat(NaN, size(a));
end
Q(a~=Inf & b==0) = 1;
Q(a~=Inf & b==Inf) = 0;
Q(a==Inf & b~=Inf) = 1;
z = (isnan(Q) & a==0 & b~=Inf);
if (any(z))
   Q(z) = exp((-b(z).^2)./2);
end

z = isnan(Q) & ~isnan(a) & ~isnan(b);
if (any(z(:)))
%    aa = (a(z).^2)./2;
   aa = (a.^2)./2;
   bb = (b(z).^2)./2;

   d = exp(-aa);
   h = d;
   f = bb.*exp(-bb);
   k = 1;
   delta = f .* h;
   sum = delta;
   j = (delta > sum.*eps(class(delta)));
   while any(j)
      d = aa.*d./k;
      h = h + d;
      f = bb.*f./(k+1);
      delta = f .* h;
      sum(j) = sum(j) + delta(j);
      j = (delta > sum.*eps(class(delta)));
      k = k + 1;
   end
   Q(z) = 1 - sum;
end


function X = FDDL_updateX(Y, Y_range, D, D_range, X, opts)
    % X = argmin_X 0.5\|Yhat - Dhat X\| +
    %    + 0.5*lambda2(\sum (normF2(Xi - Mi) - normF2(Mi - M)) + normF2(X) + normF2(X0 - M0))
    %    + lambda1*norm1(X)
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% ================== block: test module ==========================
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if nargin == 0
        addpath('../utils');
        addpath('../sparse_coding');
        tic
        d       = 30;
        N       = 7;
        k       = 5;
        C       = 3 ;
        Y       = normc(rand(d,N*C));
        D       = normc(rand(d,k*C));
        Y_range = N* (0:C);
        D_range = k* (0:C);
        X       = randn(size(D,2), size(Y,2));

        opts.k0       = k0;
        opts.lambda1  = 0.01;
        opts.lambda2  = 0.002;
        opts.lambda3  = 0.1;
        opts.max_iter = 250;
        opts.show     = true;
        opts.check_grad = true;
        opts          = initOpts(opts); % other attributes
    end

    lambda1  = opts.lambda1;
    lambda2  = opts.lambda2;
    DtD      = D'*D;
    D_0      = buildMhat(DtD, D_range, D_range);
    Dhat     = D_0 + 2*opts.lambda2*eye(size(D_0,1));
    DtY     = D'*Y;
    Y_0     = buildMhat(DtY, D_range, Y_range);
    %% cost w.r.t. X, X0, not includeing norm1 terms
    function cost = calc_f(X)
        cost = 0.5*(normF2(Y - D*X) + ...
                    FDDL_fidelity(Y, Y_range, D, D_range, X)) + ...
               0.5*opts.lambda2* (FDDL_discriminative(X, Y_range));
    end
    %% Total cose
    function cost = calc_F(X)
        cost = calc_f(X) + lambda1*norm1(X);
    end
    %%
    %% Gradient for FISTA
    function g = grad(X)
        g       = Dhat*X - Y_0 + buildM_2Mbar(X, Y_range, lambda2);
    end
    %% check gradient
    if opts.check_grad &&~check_grad(@calc_f, @grad, X)
        fprintf('Check gradient or cost again!\n')
        pause
    end
    %% ========= Main FISTA ==============================
    optsXX0          = opts;
    optsXX0.max_iter = 300;
    L = max(eig(Dhat)) + 6*lambda2;
    X      = fista(@grad, X, L, opts.lambda1, opts, @calc_F);
    %%
    if nargin == 0
        fprintf('done, press any key to see results\n');
        pause;
    end
end


function g = rbfbkp(net, x, z, n2, deltas)
%RBFBKP	Backpropagate gradient of error function for RBF network.
%
%	Description
%	G = RBFBKP(NET, X, Z, N2, DELTAS) takes a network data structure NET
%	together with a matrix X of input vectors, a matrix  Z of hidden unit
%	activations, a matrix N2 of the squared distances between centres and
%	inputs, and a matrix DELTAS of the  gradient of the error function
%	with respect to the values of the output units (i.e. the summed
%	inputs to the output units, before the activation function is
%	applied). The return value is the gradient G of the error function
%	with respect to the network weights. Each row of X corresponds to one
%	input vector.
%
%	This function is provided so that the common backpropagation
%	algorithm can be used by RBF network models to compute gradients for
%	the output values (in RBFDERIV) as well as standard error functions.
%
%	See also
%	RBF, RBFGRAD, RBFDERIV
%

%	Copyright (c) Ian T Nabney (1996-2001)

% Evaluate second-layer gradients.
gw2 = z'*deltas;
gb2 = sum(deltas);

% Evaluate hidden unit gradients
delhid = deltas*net.w2';

gc = zeros(net.nhidden, net.nin);
ndata = size(x, 1);
t1 = ones(ndata, 1);
t2 = ones(1, net.nin);
% Switch on activation function type
switch net.actfn

case 'gaussian' % Gaussian
   delhid = (delhid.*z);
   % A loop seems essential, so do it with the shortest index vector
   if (net.nin < net.nhidden)
      for i = 1:net.nin
         gc(:,i) = (sum(((x(:,i)*ones(1, net.nhidden)) - ...
            (ones(ndata, 1)*(net.c(:,i)'))).*delhid, 1)./net.wi)';
      end
   else
      for i = 1:net.nhidden
         gc(i,:) = sum((x - (t1*(net.c(i,:)))./net.wi(i)).*(delhid(:,i)*t2), 1);
      end
   end
   gwi = sum((n2.*delhid)./(2.*(ones(ndata, 1)*(net.wi.^2))), 1);

case 'tps'	% Thin plate spline activation function
   delhid = delhid.*(1+log(n2+(n2==0)));
   for i = 1:net.nhidden
      gc(i,:) = sum(2.*((t1*(net.c(i,:)) - x)).*(delhid(:,i)*t2), 1);
   end
   % widths are not adjustable in this model
   gwi = [];
case 'r4logr' % r^4 log r activation function
   delhid = delhid.*(n2.*(1+2.*log(n2+(n2==0))));
   for i = 1:net.nhidden
      gc(i,:) = sum(2.*((t1*(net.c(i,:)) - x)).*(delhid(:,i)*t2), 1);
   end
   % widths are not adjustable in this model
   gwi = [];
otherwise
   error('Unknown activation function in rbfgrad')
end

g = [gc(:)', gwi, gw2(:)', gb2];


function I = randpermNK(N,K)
% N is the number of trials/subjects
% K is the number of groups
if K==1
    I = cell(1);
    I{1} = randperm(N);
    return;
end
np=(N-mod(N,K))/K; % number of elements per group
[~,idx]=sort(rand(N,1));
i=1;
j=1;
I={};
while 1
    I{j}=idx(i:i+np-1,1);
    if N-(i+np)+1 < np
        I{j} = [I{j}; idx(i+np:end,1)];
        break
    end
    i=i+np;
    j=j+1;
end
end


function C = PruneTree(C)

% Logic:
% Start with a clique, scan through its neighbors. If you find a neighbor
% such that it is a superset of the clique you started with, then you know
% that you can prune the tree. For instance, let's take the following
% clique tree:
% ABE -- AB ---AD
% Let's say we started with AB. We scan through its neighbors and find that
% AB is a subset of ABE. So we cut off the edges connected to AB and add an
% edge between ABE and all of AB's other neighbors. This maintains the
% running intersection property and gives us a more compact clique tree
% which looks like: ABE -- AD.
%
% Copyright (C) Daphne Koller, Stanford University, 2012

toRemove = [];

for i=1:length(C.nodes)

    if ismember(i,toRemove), continue, end;
    neighborsI = find(C.edges(i,:));

    for c = 1: length(neighborsI),

        j = neighborsI(c);
        assert(i ~= j);

        if ismember(j,toRemove), continue, end;

        if (sum(ismember(C.nodes{i}, C.nodes{j})) == length(C.nodes{i}))

            for nk = neighborsI

                % find neighbors and connect with that.
                if length(intersect(C.nodes{i}, C.nodes{nk})) == length(C.nodes{i})
                    C.edges(setdiff(neighborsI,[nk]),nk) = 1;
                    C.edges(nk,setdiff(neighborsI,[nk])) = 1;
                    break;
                end
            end

            % kill the edges for the clique that is to be removed.
            C.edges(i,:) = 0;
            C.edges(:,i) = 0;
            toRemove = [i toRemove];

        end
    end
end

toKeep = setdiff(1:length(C.nodes),toRemove);

C.nodes(toRemove) = [];

if isfield(C, 'edges')
    C.edges = C.edges(toKeep,toKeep);
else
    C.edges = [];
end

end


function h = drawPoint3d(varargin)
%DRAWPOINT3D Draw 3D point on the current axis.
%
%   drawPoint3d(X, Y, Z)
%   will draw points defined by coordinates X, Y and Z.
%   X, Y and Z are N*1 array, with N being number of points to be drawn.
%
%   drawPoint3d(COORD) packs coordinates in a single [N*3] array.
%
%   drawPoint3d(..., OPT) will draw each point with given option. OPT is a
%   string compatible with 'plot' model.
%
%   drawPoint3d(AX,...) plots into AX instead of GCA.
%
%   H = drawPoint3d(...) returns a handle H to the line object
%
%   Example
%     % generate points on a 3D circle
%     pts = circleToPolygon([40 30 20], 120);
%     mat = eulerAnglesToRotation3d([30 20 10]);
%     pts3d = transformPoint3d([pts zeros(120,1)],mat);
%     figure; drawPoint3d(pts3d, 'b.');
%     view(3); axis equal;
%
%   See also
%     points3d, clipPoints3d, drawPoint
%

% ---------
% Author : David Legland
% INRA - TPV URPOI - BIA IMASTE
% created the 18/02/2005.
%
%   HISTORY
%   04/01/2007: remove unused variables, and enhance support for plot
%       options
%   12/02/2010 does not clip points anymore
%   12/01/2018 added axes handle input
%

if numel(varargin{1}) == 1 && ishghandle(varargin{1}, 'axes')
    hAx = varargin{1};
    varargin(1)=[];
else
    hAx = gca;
end

if length(varargin) == 1 && size(varargin{1}, 2) == 3
    % points are given as one single array with 3 columns
    px = varargin{1}(:,1);
    py = varargin{1}(:,2);
    pz = varargin{1}(:,3);
    varargin = {};
elseif length(varargin) == 2 && size(varargin{1}, 2) == 3
    % points are given as one single array with 3 columns
    px = varargin{1}(:,1);
    py = varargin{1}(:,2);
    pz = varargin{1}(:,3);
    varargin = varargin(2);
elseif length(varargin) >= 3 && size(varargin{1}, 2) == 3
    % points are given as one single array with 3 columns
    px = varargin{1}(:,1);
    py = varargin{1}(:,2);
    pz = varargin{1}(:,3);
    varargin = varargin(2:end);
elseif length(varargin) == 3 && numel(varargin{1})==numel(varargin{2}) && numel(varargin{1})==numel(varargin{3})
    % points are given as 3 columns with equal lengths
    px = varargin{1};
    py = varargin{2};
    pz = varargin{3};
    varargin = {};
elseif length(varargin) > 3
    % points are given as 3 columns with equal lengths
    px = varargin{1};
    py = varargin{2};
    pz = varargin{3};
    varargin = varargin(4:end);
else
    error('wrong number of arguments in drawPoint3d');
end

% default draw style: no line, marker is 'o'
if length(varargin) ~= 1
    varargin = ['linestyle', 'none', 'marker', 'o', varargin];
end

% plot only points inside the axis.
hh = plot3(hAx, px, py, pz, varargin{:});

if nargout > 0
    h = hh;
end


% FindInl    find inliers in joint image matrix
%			 by pairwise epipolar geometry
%
% function IdMatIn = findinl(Ws,IdMat,tol)
% Ws ... 3MxN joint image matrix
% IdMat ... MxN ... 0 -> no point detected
%                   1 -> point detected
% tol ... [pixels] tolerance for the epipolar geometry
%         the point are accpted as outliers only if they
%         are closer to the epipolar line than tol

% $Author: svoboda $
% $Revision: 2.1 $
% $Id: findinl.m,v 2.1 2003/07/30 10:28:29 svoboda Exp $
% $State: Exp $

function IdMatIn = findinl(Ws,IdMat,tol)

NoCams = size(IdMat,1);

% fill the array of structures not_used denoted as 0
% allocate the array of structures for used
for i=1:NoCams,
  not_used(i).pts = sum(IdMat(i,:));
  used(i).pts	  = -1;
end

% allocate IdMat for outliers
IdMatIn = zeros(size(IdMat));

while (sum([not_used.pts])>1-NoCams),
  [buff, id.cam_max]  = max([not_used.pts]);
  used	   = add(used, id.cam_max, not_used(id.cam_max).pts);
  not_used = remove(not_used, id.cam_max);
  Mask	   = repmat(IdMat(id.cam_max,:),NoCams,1);
  Corresp  = Mask & IdMat;
  Corresp(id.cam_max,:) = 0;
  [buff, id.cam_to_pair] = max(sum(Corresp')); % find the camera with most correspondences
  idx.corr_to_pair = find(sum(IdMat([id.cam_max,id.cam_to_pair],:))==2);
  % used	   = add(used, id.cam_to_pair, not_used(id.cam_to_pair).pts);
  % not_used = remove(not_used, id.cam_to_pair);
  if size(idx.corr_to_pair,2)<8,
	error('Not enough points to compute epipolar geometry in RANSAC validation')
  end
  Wspair   = [];
  Wspair   = Ws(id.cam_max*3-2:id.cam_max*3, idx.corr_to_pair);
  Wspair   = [Wspair; Ws(id.cam_to_pair*3-2:id.cam_to_pair*3, idx.corr_to_pair)];
  % id
  [F, inls] = rEG(Wspair,tol,tol,0.99);
  IdMatIn(id.cam_max, idx.corr_to_pair(inls)) = 1;
  IdMat(id.cam_max, :)						  = 0;
  IdMat(id.cam_max, idx.corr_to_pair(inls))	  = 1;
end

function list = add(list, id, value)
list(id).pts = value;
return

function list = remove(list, id)
list(id).pts = -1;
return


function w = ymdf_to_weekday_gregorian ( y, m, d, f )

%*****************************************************************************80
%
%% YMDF_TO_WEEKDAY_GREGORIAN returns the weekday of a Gregorian YMDF date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, M, D, real F, the YMDF date.
%
%    Output, integer W, is the week day number of the date, with
%    1 for Sunday, through 7 for Saturday.
%
  jed = ymdf_to_jed_gregorian ( y, m, d, f );

  [ w, f2 ] = jed_to_weekday ( jed );

  return
end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% Tutorial for FAIR: Examples for grid generation
%
%==============================================================================

omega = [0,6,0,4,0,8]
m     = [3,2,2]
xc    = getCellCenteredGrid(omega(1:2),m(1));        xc = reshape(xc,1,[])
xc    = getCellCenteredGrid(omega(1:4),m(1:2));      xc = reshape(xc,[m(1:2),2])
xc    = getCellCenteredGrid(omega(1:6),m(1:3));      xc = reshape(xc,[m(1:3),3])
%==============================================================================


function [Nm] = cal2Nm(cal)
% Convert energy or work from calories to newtons-meters.
% Note: these calories are different from the capitalized Calories found on
% American cereal boxes.  American consumers are to healthy to eat a
% 250,000 calorie candy bar.
% Chad A. Greene 2012
Nm = cal*4.1868;


 function subplot_stack(x, ys, str_title, colors)
%function subplot_stack(x, ys, str_title, colors)
% a tight stack of subplots to show L signal components
% in
%	x	[N,1]
%	ys	[N,L]		or ?

if nargin == 1 && streq(x, 'test'), subplot_stack_test, return, end
if nargin < 2, ir_usage, end
if ~isvar('colors') || isempty(colors), colors = {'c', 'y'}; end
if ~isvar('str_title') || isempty(str_title)
	str_title = '';
end

if ~iscell(ys)
	ys = {ys};
end
L = size(ys{1},2);

apos = get(gca, 'position'); % current axes position
for ll=1:L
%	pos = [0.1 0.1+0.8/L*(L-ll) 0.8 0.8/L];
	pos = [apos(1) apos(2)+apos(4)/L*(L-ll) apos(3) apos(4)/L];
	subplot('position', pos) % l b w h
	for ip=1:length(ys)
		plot(	x, real(ys{ip}(:,ll)), colors{1+2*(ip-1)}, ...
			x, imag(ys{ip}(:,ll)), colors{2+2*(ip-1)})
		if ip == 1, hold on, end
	end
	hold off
	axis tight
	ytick(0), set(gca, 'yticklabel', '')
	fontsize = 10;
	fontweight = 'normal';
	texts(1.02, 0.8, sprintf('%d.', ll), ...
		'fontsize', fontsize, 'fontweight', fontweight)

	if ll==1, title(str_title), end
	if ll<L
		xtick off
	end
end

function subplot_stack_test
x = linspace(0,1,101)';
y = exp(2i*pi*x*[1:5]);
if im
	clf, subplot(121)
	subplot_stack(x, y)
end


I = vl_test_pattern(1) ;
ur = 1:size(I,2) ;
vr = 1:size(I,1) ;

[u,v] = meshgrid(ur(1:5:end),vr(1:5:end)) ;

f = [u(:)';v(:)'] ;
K = size(f,2) ;
f = [f ; 2 * ones(1,K) ; 0 * ones(1,K)] ;

f = vl_sift(single(I), 'frames', f, 'orientations') ;

%f = diag([1 1 6 1]) * f ;

figure(1) ; clf ;
imagesc(I) ; colormap gray ; hold on ;
vl_plotframe(f,'color','k','linewidth',3) ;
vl_plotframe(f,'color','y','linewidth',2) ; axis equal ; axis off ;

vl_demo_print('sift_or') ;


function [] = bandfiltering(z_grid,x_res,y_res,spatial_bands,save_path,ifg_based_correction,n_degree_butterworth,norm_filter_flag)
% function that computes the band pass filtered data usigng 2D FFT and a
% butterwurth function. Make sure the input data is on a regular grid and
% has no NaN values. There is no output passed in this function. Instead
% the bandfiltered data is saved for each dataset individually, e.g.
% 'dataset_1.mat' for the first dataset and increasing for the others.
% input:
% z_grid                    The data specified as a grid, i.e. a matrix.
%                           Additonal datasets can be specified by
%                           increasing the third dimention.
% x_res                     The resolution in x-direction in m of the grid
% y_res                     The resolution in y-direction in m of the grid
% spatial_bands             The spatial band that need to be filtered in m,
%                           specified as a 2 column matrix [lower upper].
%                           Multiple band filters can be specified by
%                           increasing the number or rows.
% Optional inputs:
% save_path                 The path were the output data will be saved. default
%                           is the current directory
% n_degree_butterworth      The degree of the butterwurth filter, by
%                           default this is set to be 3.
% norm_filter_flag          Normalisation of the butterwurth filter is done
%                           by default. This is to scope with the issue
%                           when the extremes of the bandfitler are too
%                           close to eachother causing only a partial
%                           amplitude passing in the selected band.
%
%     Copyright (C) 2015  Bekaert David - University of Leeds
%     Email: eedpsb@leeds.ac.uk or davidbekaert.com
%
%     This program is free software; you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation; either version 2 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License along
%     with this program; if not, write to the Free Software Foundation, Inc.,
%     51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
%
% By Bekaert David - December 2012
% modifications:
% DB	02/2013		Include the save_path variable
% DB    03/2013     Include warnings for edge effects and to large spatial
%                   bandfilters.
% DB    03/2013     Include 1D filtering to cope with limitation of narrow
%                   datasets
% DB    05/2013     Include option to crop out a region

% optional inputs for checking results:
% figure properties
fontsize = 15;
plot_figures = 0;          % plot the figures for the n_plot_dataset dataset
save_fig =0;                % when 1 save figures in the figures folder in tha aps_p folder.
n_plot_dataset=1;           % plot the figures for this band dataset.
                            % Topography is first then the interferograms
check_flag = 0;             % some additional figures being generated when turn on
mirror_flag = 1;            % when 1, mirror the dataset such filtering effects
                            % are reduced. The mirror is based on the
                            % largest spatial filter wavelength.

max_perc_mirror_2D = 100;    % filtersize with respect to the dataset spatial
                            % dimension in percentage till what 2D
                            % filtering is allowed. When larger 1D
                            % filtering is performed in the larger
                            % dimension of X or Y.

warning_flag_perc = 10;     % Output a warning when more no mirroring is done,
                            % and the percentage of half the filter length
                            % with respect to the maximum dimension is mirrored.

warning_mirror_flag_perc = 50;
                            % Output a warning when more than this percentage
                            % with respect to the maximum dimension is mirrored.
                            % Mirroring is done by half the maximum filter size.

% getting the data from the parms_aps file
crop_flag = getparm_aps('crop_flag');
 % setting the function defaults
if nargin<6
    ifg_based_correction='n';
end
if nargin<7
    n_degree_butterworth = [];
end
if nargin<8
    norm_filter_flag = [];
end
if isempty(n_degree_butterworth)==1
   n_degree_butterworth=3;
end
if isempty(norm_filter_flag)==1
   norm_filter_flag=1;
end
if nargin<5 || isempty(save_path)==1
   save_path = './';
end
fprintf(['***Bandfiltering***\n'])
fprintf(['Using butterworth filter degree: ' num2str(n_degree_butterworth) '\n'])
if norm_filter_flag==1
    fprintf(['Normalise the butterworth filter. \n'])
end

norm_filter_flag=0
n_degree_butterworth = 10
% number of datasets
n_datasets = size(z_grid,3);
n_band_filters = size(spatial_bands,1);
if strcmp(ifg_based_correction,'y')
    h_ifg_number = n_datasets/2;
else
    h_ifg_number=1;
end

if size(spatial_bands,1)==1 && size(spatial_bands,2)==2 && spatial_bands(1,1)==0 && spatial_bands(1,2)==inf
    % this is a band filter of the whole image
        for k=1:n_datasets

            if k<=h_ifg_number && h_ifg_number~=1
                % these are interferograms
                save_name = ['bandfilter_regular_hgt_ifg_' num2str(k) '.mat'];
            elseif k<=h_ifg_number && h_ifg_number==1
                % these are the heights
                save_name = 'bandfilter_regular_hgt.mat';
            else
                % thse are interferograms
                 save_name = ['bandfilter_regular_ifg_' num2str(k-h_ifg_number) '.mat'];
            end
            data_band_out(:,:) = z_grid(:,:,k);
            clear data_band

            dimension_filter=NaN;
            % saving the bandfiltered data for eacht dataset seperately
            save([save_path, filesep, save_name],'data_band_out','x_res','y_res','spatial_bands','dimension_filter')
            clear data_band_out
        end
else

    % Starting the bandfiltering code
    if plot_figures==1
        if n_plot_dataset<=h_ifg_number && h_ifg_number~=1
            % thse are interferograms
            save_folder_str=['aps_p' filesep 'fig_bandfilter_hgt_ifg' num2str(n_plot_dataset)];
        elseif n_plot_dataset<=h_ifg_number && h_ifg_number==1
            % these are the heights
            save_folder_str=['aps_p' filesep 'fig_bandfilter_hgt'];
        else
            % these are interferograms
            save_folder_str=['aps_p' filesep 'fig_bandfilter_ifg_' num2str(n_plot_dataset-h_ifg_number)];
        end

    %     if n_plot_dataset==1
    %         save_folder_str=['aps_p' filesep 'fig_bandfilter_hgt'];
    %     else
    %         save_folder_str=['aps_p' filesep 'fig_bandfilter_ifg_' num2str(n_plot_dataset-1)];
    %     end
        if exist(save_folder_str,'dir')~=7
           mkdir(save_folder_str)
        end
    end

    % original gridsize
    data_rows_or = size(z_grid,1);
    data_columns_or = size(z_grid,2);

    % investigate in the percentage of padding that is being performed
    % below the percentage of the maximum band filter with respect to the dimension is computed
    x_samples_overlap = ceil(max(spatial_bands,[],2)./x_res/2);
    x_perc_overlap = x_samples_overlap./data_columns_or*100;
    y_samples_overlap = ceil(max(spatial_bands,[],2)./y_res/2);
    y_perc_overlap = y_samples_overlap./data_rows_or*100;

    % output a warning in case the padding is to big or when the the filter
    % effect hit the warning percentage as set at the start of the code.
    if mirror_flag==1
       warning_perc = warning_mirror_flag_perc ;
    else
       warning_perc = warning_flag_perc ;
    end
    ix_x = find(x_perc_overlap>warning_perc);
    ix_y = find(y_perc_overlap>warning_perc);
    ix = intersect(ix_x,ix_y);
    clear ix_x ix_y
    if isempty(ix)~=1
        new_linestr = repmat('\n',length(ix),1);
        outputstr = [num2str(spatial_bands(ix,:)) new_linestr];
        if mirror_flag==1
            fprintf(['***Warning: The following band filters are mirrored about ',num2str(warning_perc),' perc of the maximum dimension: \n'])
            for kk=1:size(outputstr,1)
                fprintf(outputstr(kk,:)) ;
            end
            clear outputstr kk
            fprintf(['This migth introduce arctifacts. \n'])
            fprintf(['It is recommended to limit to smaller spatial bandwidths. \n'])

        else
            fprintf(['***Warning: The following band filters are above ',num2str(warning_perc),' perc of the maximum dimension: \n'])
            for kk=1:size(outputstr,1)
                fprintf(outputstr(kk,:)) ;
            end
            clear outputstr kk
            fprintf(['Likely edge effects are introduced. \n'])
            fprintf(['Turn the mirror flag on and/or limit to smaller spatial bandwidths. \n\n'])
        end
    end
    clear ix

    % output information on which bandfilters are replaced by a 1D filter to
    % reduce edge effects from a limiting dimension
    % x/y_perc_overlap represents the maximum bandfilter size with respect to
    % the x/y dimension given as a percentage
    ix_x = find(x_perc_overlap>max_perc_mirror_2D);
    ix_y = find(y_perc_overlap>max_perc_mirror_2D);

    if isempty(ix_x)~=1 || isempty(ix_y)~=1
        x_n_cases = length(ix_x);
        y_n_cases = length(ix_y);
        if x_n_cases>y_n_cases
             fprintf(['X-direction appears limited for band filtering. \n'])
             fprintf(['1D (Y-direction) band filtering is performed for the following bands: \n'])
             new_linestr = repmat('\n',length(ix_x),1);
             outputstr = [num2str(spatial_bands(ix_x,:)) new_linestr];
             for kk=1:size(outputstr,1)
                 fprintf(outputstr(kk,:)) ;
             end

             % setting the variables for the 1D filtering
             filter_1D_Y = 1;
             filter_1D_X = 0;
             ix_1D_filter = ix_x;
             clear outputstr new_linestr kk ix_y ix_x

        elseif y_n_cases>x_n_cases
             fprintf(['Y-direction appears limited for band filtering. \n'])
             fprintf(['1D (X-direction) band filtering is performed for the following bands: \n'])
             new_linestr = repmat('\n',length(ix_y),1);
             outputstr = [num2str(spatial_bands(ix_y,:)) new_linestr];
             for kk=1:size(outputstr,1)
                 fprintf(outputstr(kk,:)) ;
             end

             % setting the variables for the 1D filtering
             filter_1D_Y = 0;
             filter_1D_X = 1;
             ix_1D_filter = ix_y;
             clear outputstr new_linestr kk ix_x ix_y

        else
             fprintf(['Both dataset appears as limited for band filtering. \n'])
             fprintf(['2D band filtering is performed, but check the following bands: \n'])
             new_linestr = repmat('\n',length(ix_y),1);
             outputstr = [num2str(spatial_bands(ix_y,:)) new_linestr];
             for kk=1:size(outputstr,1)
                 fprintf(outputstr(kk,:)) ;
             end

             % setting the variables for the 1D filtering
             filter_1D_Y = 0;
             filter_1D_X = 0;
             ix_1D_filter = [];
             clear outputstr new_linestr kk ix_x ix_y
        end
    else
         filter_1D_Y = 0;
         filter_1D_X = 0;
         ix_1D_filter = [];
    end

    if plot_figures==1
        data_original_image = z_grid(:,:,n_plot_dataset);
    end

    % mirror the edges of the grid to reduce filter effects at the edges
    % get the maximum size of the spatial filters
    if mirror_flag == 1
        fprintf('Perform mirroring to reduce filtering effects on edges \n')
        fprintf('By half the maximum filter length at each edge. \n')

        % maximum spatial wavelength
        max_band = max(max(spatial_bands));
        if max_band == inf
           max_band = 100000;        % extend the grid to a maximum of 100 km in case one goes to infinite
        end
        n_mirror_x = ceil(max_band./x_res*1.5);
        n_mirror_y = ceil(max_band./y_res*1.5);

        for k=1:n_datasets
            % padding the grid symmetric
            z_grid_new(:,:,k) = padarray(z_grid(:,:,k),[n_mirror_y n_mirror_x],'symmetric');
            % plotting an intermediate figure when requested
            if plot_figures==1 && k==n_plot_dataset
                h1= figure('name','Data symmetric padded for largest filter');
                imagesc(z_grid_new(:,:,k))
                axis equal
                axis tight

                % saving of the figure when requested
                if save_fig==1
                    fig_save_name = [save_folder_str filesep 'original_data_mirrored.eps'];
                    set(h1,'PaperPositionMode','auto')
                    print(h1,'-depsc','-r150',fig_save_name)
                    clear fig_save_name
                    close(h1)
                end
                clear h1
            end
        end
        clear z_grid
        z_grid = z_grid_new;
    else
       fprintf('No mirroring performed. Filter artifacts will become more persistent for larger wavelengths! \n')
    end

    % size of the dataset
    data_rows = size(z_grid,1);
    data_columns = size(z_grid,2);

    % Sampling frequency follows from the resolution
    fs_rows = 1/y_res;          % rows sampling frequency [1/m]
    fs_columns = 1/x_res;   	% columns sampling frequency [1/m]

    % rows and columns of the data such they are a power of 2
    % this will speed up the fft and will autmoatically padd
    % the data matrix with zeros where needed
    data_rows_new = 2.^nextpow2(data_rows);
    data_columns_new = 2.^nextpow2(data_columns);

    if plot_figures==1
        if mirror_flag==1
            % computation of the axis extremes based on the resolution given
            X_lims_fig = [0 data_columns_or*x_res];     % axis limits in [m]
            Y_lims_fig = [0 data_rows_or*y_res];        % axis limits in [m]
        else
            X_lims_fig = [0 data_columns*x_res];        % axis limits in [m]
            Y_lims_fig = [0 data_rows*y_res];           % axis limits in [m]
        end
        % computation of the axis extremes based on the resolution given
        % Asummed to be symmetric padded. In case no padding is done, this will
        % be equal to the orginal dataset
        X_lims_fig_syix_1D_filterm = [0 data_columns*x_res];        % axis limits in [m]
        Y_lims_fig_sym = [0 data_rows*y_res];           % axis limits in [m]
    end

    % Loop over each dataset. Invert to the freq domain once and reinvert for
    % each bandfilter. i.e. There will be a loop over the different
    % bandfilters. For each dataset the data is being saved in a matfile.
    for k=1:n_datasets
        if k==n_plot_dataset && plot_figures==1
            plot_fig_flag = 1;
        else
            plot_fig_flag = 0;
        end

        fprintf(['Progress: ' num2str(k) '/' num2str(n_datasets) ,' done \n'])
        % when chosen visualise the dataset
        if plot_fig_flag==1
            h1 = figure('name','Original image');
            imagesc(X_lims_fig,Y_lims_fig,data_original_image)
            cc = colorbar;
            axis xy
            axis equal
            axis tight
            set(gca,'fontsize',fontsize)
            xlabel('Distance [m]','fontsize',fontsize)
            ylabel('Distance [m]','fontsize',fontsize)
            title('Original image','fontsize',fontsize)
            colorlimits = get(cc,'YLim');

            % saving of the figure when requested
            if save_fig==1
                fig_save_name = [save_folder_str filesep 'input.eps'];
                set(h1,'PaperPositionMode','auto')
                print(h1,'-depsc','-r150',fig_save_name)
                clear fig_save_name
                close(h1)
            end
            clear h1

        end

        % converting to the frequency domain by FFT
        data_freq_complex = fft2(z_grid(:,:,k),data_rows_new,data_columns_new);

        % shifting the spectrum around zero freq
        % matlab shifts with 1 pixel off for even number columns and rows
        data_freq_complex = fftshift(data_freq_complex);
        data_freq_complex_new = [data_freq_complex  data_freq_complex(:,1)];
        data_freq_complex_new = [data_freq_complex_new ; data_freq_complex_new(1,:)];
        clear data_freq_complex

        % Frequency domain: sampling resolution follows from the Nyquist requency (spampling freq/samples)
        % freq resolution of the bins in the freq domain
        fs_rows = 1/data_rows_new*1/y_res;
        fs_columns = 1/data_columns_new*1/x_res;

        % Computing the frequencies for the axis in the freq domain figures.
        freq_rows_fig_vector = [-(data_rows_new-1)/2-0.5:1:(data_rows_new-1)/2+0.5].*fs_rows;
        freq_columns_fig_vector= [-(data_columns_new-1)/2-0.5:1:(data_columns_new-1)/2+0.5].*fs_columns;

        if plot_fig_flag==1  && check_flag==1
            figure('name','Spectrum centralised around zero freq')
            imagesc(freq_columns_fig_vector,freq_rows_fig_vector,abs(data_freq_complex_new))
            xlabel('Xfreq [1/m]','fontsize',fontsize)
            ylabel('Yfreq [1/m]','fontsize',fontsize)
            title('Spectrum centralised around zero freq','fontsize',fontsize)
            colorbar
            set(gca,'fontsize',fontsize)
            axis equal
            axis tight

            figure('name','Phase centralised around zero freq')
            imagesc(freq_columns_fig_vector,freq_rows_fig_vector,angle(data_freq_complex_new))
            xlabel('Xfreq [1/m]','fontsize',fontsize)
            ylabel('Yfreq [1/m]','fontsize',fontsize)
            title('Phase centralised around zero freq','fontsize',fontsize)
            colorbar
            set(gca,'fontsize',fontsize)
            axis equal
            axis tight
        end

        %% Computing the grid of frequencies
        freq_rows_fig_matrix = repmat(freq_rows_fig_vector',1,data_columns_new+1);
        freq_columns_fig_matrix = repmat(freq_columns_fig_vector,data_rows_new+1,1);
        % Set the frequencies for 1D or 2D filtering
        if filter_1D_X==1
            % 1D X-direction filter
            freq_fig_matrix_1D = freq_columns_fig_matrix;
            if plot_fig_flag==1 && check_flag==1
                figure('name','Frequency grid 1D X-direction filter')
                imagesc(freq_fig_matrix_1D)
                xlabel('X','fontsize',fontsize)
                ylabel('Y','fontsize',fontsize)
                cc = colorbar;
                set(gca,'fontsize',fontsize)
                xlabel(cc,'Freq [1/m]','fontsize',fontsize)
            end
        elseif filter_1D_Y==1
            % 1D Y-direction filter
            freq_fig_matrix_1D = freq_rows_fig_matrix;
            if plot_fig_flag==1 && check_flag==1
                figure('name','Frequency grid 1D Y-direction filter')
                imagesc(freq_fig_matrix_1D)
                xlabel('X','fontsize',fontsize)
                ylabel('Y','fontsize',fontsize)
                cc = colorbar;
                set(gca,'fontsize',fontsize)
                xlabel(cc,'Freq [1/m]','fontsize',fontsize)
            end
        end
        % 2D filter
        freq_fig_matrix_2D = sqrt(freq_rows_fig_matrix.^2+freq_columns_fig_matrix.^2);
        if plot_fig_flag==1 && check_flag==1
            figure('name','Frequency grid')
            imagesc(freq_fig_matrix_2D)
            xlabel('X','fontsize',fontsize)
            ylabel('Y','fontsize',fontsize)
            cc = colorbar;
            set(gca,'fontsize',fontsize)
            xlabel(cc,'Freq [1/m]','fontsize',fontsize)
        end

        %% Loop over the different bandfilters
        % initialisation of the output data_band variable
        % This is after the symmetric padding has been removed again.
        data_band_out = NaN([data_rows_or data_columns_or n_band_filters]);
        dimension_filter = NaN([n_band_filters 1]);
        for kk=1:n_band_filters
            % output to the screen
            fprintf(['Spatial bandfilter: ',num2str(spatial_bands(kk,1)), '\t - ',num2str(spatial_bands(kk,2)),' \t m \t'])

            % convert spatial wavelength band to spatial frequency band
            f_band = 1./sort(spatial_bands(kk,:));	% frequency band [1/m]

            % Checking if the band filter is set as 1D or 2D filter
            filter_1D = find(kk==ix_1D_filter);
            if isempty(filter_1D)~=1
                % do 1D filtering
                % computing the function of the bandfilter
                H_low = 1./(1+(freq_fig_matrix_1D./f_band(1)).^(2*n_degree_butterworth));
                H_high = 1./(1+(freq_fig_matrix_1D./f_band(2)).^(2*n_degree_butterworth));
                dimension_filter(kk) = 1;
                fprintf(['(1D filter)\n'])
            else
                % do 2D filtering
                % computing the function of the bandfilter
                H_low = 1./(1+(freq_fig_matrix_2D./f_band(1)).^(2*n_degree_butterworth));
                H_high = 1./(1+(freq_fig_matrix_2D./f_band(2)).^(2*n_degree_butterworth));
                dimension_filter(kk) = 2;
                fprintf(['(2D filter)\n'])
            end

            % Correct for the case of a filter having inf in its range
            ix_center_spectrum = (size(H_high)-1)./2+1;
            if isnan(H_high(ix_center_spectrum(1),ix_center_spectrum(2)))
                H_high(ix_center_spectrum(1),ix_center_spectrum(2))=0;
            end
            if isnan(H_low(ix_center_spectrum(1),ix_center_spectrum(2)))
                H_low(ix_center_spectrum(1),ix_center_spectrum(2))=0;
            end
            % Computing the band pass filter
            H_Butterworth =  (H_low - H_high);
            H_Butterworth_norm = H_Butterworth./max(max(H_Butterworth));

            % plotting when requested
            if plot_fig_flag==1
                % plotting the filter around zero for the rows
                h1 = figure('name','Rows freq filter');
                subplot(4,1,1)
                plot(freq_rows_fig_vector,H_low(:,ix_center_spectrum(2)),'b.-')
                ylim([0 1])
                title(['freq : ' num2str(f_band(1)) ])
                subplot(4,1,2)
                plot(freq_rows_fig_vector,H_high(:,ix_center_spectrum(2)),'b.-')
                ylim([0 1])
                title(['freq : ' num2str(f_band(2)) ])
                subplot(4,1,3)
                plot(freq_rows_fig_vector,H_Butterworth(:,ix_center_spectrum(2)),'b.-')
                ylim([0 1])
                title(['Band freq' ])
                subplot(4,1,4)
                plot(freq_rows_fig_vector,H_Butterworth_norm(:,ix_center_spectrum(2)),'b.-')
                ylim([0 1])
                title(['Norm band freq' ])

                % saving of the figure when requested
                if save_fig==1
                    fig_save_name = [save_folder_str filesep 'bandfilter_' num2str(spatial_bands(kk,1)) '_'  num2str(spatial_bands(kk,2)) 'm.eps'];
                    set(h1,'PaperPositionMode','auto')
                    print(h1,'-depsc','-r150',fig_save_name)
                    clear fig_save_name
                    close(h1)
                end
                clear h1

            end

            if norm_filter_flag ==1
                H_Butterworth = H_Butterworth_norm;
            end

            % multiplying filter in frequency domain.
            % data_freq_complex_new = real(data_freq_complex_new).*H_Butterworth+1i.*angle(data_freq_complex_new);
            data_freq_complex_new_band = data_freq_complex_new.*H_Butterworth;
            if plot_fig_flag==1  && check_flag==1
                figure('name','New spectrum centralised around zero freq')
                imagesc(freq_columns_fig_vector,freq_rows_fig_vector,abs(data_freq_complex_new_band))
                xlabel('Xfreq [1/m]','fontsize',fontsize)
                ylabel('Yfreq [1/m]','fontsize',fontsize)
                title('Spectrum centralised around zero freq','fontsize',fontsize)
                colorbar
                set(gca,'fontsize',fontsize)
                axis equal
                axis tight

                figure('name','New phase centralised around zero freq')
                imagesc(freq_columns_fig_vector,freq_rows_fig_vector,angle(data_freq_complex_new_band))
                xlabel('Xfreq [1/m]','fontsize',fontsize)
                ylabel('Yfreq [1/m]','fontsize',fontsize)
                title('Phase centralised around zero freq','fontsize',fontsize)
                colorbar
                set(gca,'fontsize',fontsize)
                axis equal
                axis tight
            end

            % Shifting the spectrum such the center frequency is at the top left corner
            data_freq_complex_new_band(:,end) = [];
            data_freq_complex_new_band(end,:) = [];
            data_freq_complex_new_band = ifftshift(data_freq_complex_new_band);

            % Inverting back to the spatial domain
            data_band = ifft2(data_freq_complex_new_band,data_rows_new,data_columns_new);

            % Removing the padded region introduced when putting the number of lines and rows 2^n
            if data_rows-data_rows_new<0			% for the rows
                data_band(data_rows+1:end,:)=[];
            end
            if data_columns-data_columns_new<0		% for the columns
                data_band(:,data_columns+1:end)=[];
            end

            % Keeping only the real part
            data_band = real(data_band);

            % removing any mirrored image from the data
            if mirror_flag == 1
                data_band_or = data_band;
                data_band([1:n_mirror_y],:)=[];
                data_band([end-n_mirror_y+1:end],:)=[];
                data_band(:,[1:n_mirror_x])=[];
                data_band(:,[end-n_mirror_x+1:end])=[];
            end

            % plotting the final results when requested
            if plot_fig_flag==1
                h1= figure('name','output image');
                imagesc(X_lims_fig,Y_lims_fig,data_band)
                % limit the colorbar to 95% bounds
                data_sorted = sort(reshape(data_band,[],1));
                ix = round([0.025 0.975].*length(data_sorted));
                colorlimits = [data_sorted(ix)];
                caxis(colorlimits)
                colorbar
                set(gca,'fontsize',fontsize)
                xlabel('Distance [m]','fontsize',fontsize)
                ylabel('Distance [m]','fontsize',fontsize)
                title(['Band filter: ' , num2str(spatial_bands(kk,1)) ,' -- ' ,num2str(spatial_bands(kk,2)),' m'],'fontsize',fontsize)
                axis equal
                axis tight
                axis xy

                % saving of the figure when requested
                if save_fig==1
                    fig_save_name = [save_folder_str filesep 'output_' num2str(spatial_bands(kk,1)) '_'  num2str(spatial_bands(kk,2)) 'm.eps'];
                    set(h1,'PaperPositionMode','auto')
                    print(h1,'-depsc','-r150',fig_save_name)
                    clear fig_save_name
                    close(h1)
                end
                clear h1

            end

            % Storing the output data
            data_band_out(:,:,kk) = data_band;
            clear data_band
        end

        if k<=h_ifg_number && h_ifg_number~=1
            % thse are interferograms
            save_name = ['bandfilter_regular_hgt_ifg_' num2str(k) '.mat'];
        elseif k<=h_ifg_number && h_ifg_number==1
            % these are the heights
            save_name = 'bandfilter_regular_hgt.mat';
        else
            % thse are interferograms
             save_name = ['bandfilter_regular_ifg_' num2str(k-h_ifg_number) '.mat'];
        end

        % saving the bandfiltered data for eacht dataset seperately
        save([save_path, filesep, save_name],'data_band_out','x_res','y_res','spatial_bands','n_degree_butterworth','norm_filter_flag','dimension_filter')
        clear data_band_out
    end
end


function batch = propagate_batch(model, batch, layer)
% propagate the batch from the bottom to the layer specified.
% also works for layer == 2.

global kConv_forward2;
global kConv_forward_c;

for l = 2 : layer - 1
    if l == 2
        stride = model.layers{l}.stride;
        hidden_presigmoid = myConvolve2(kConv_forward2, batch, model.layers{l}.w, stride, 'forward');
        hidden_presigmoid = bsxfun(@plus, hidden_presigmoid, permute(model.layers{l}.c, [2,3,4,5,1]));
        batch = 1 ./ (1 + exp(-hidden_presigmoid));
    elseif strcmp(model.layers{l}.type, 'convolution')
        stride = model.layers{l}.stride;
        hidden_presigmoid = myConvolve(kConv_forward_c, batch, model.layers{l}.w, stride, 'forward');
        hidden_presigmoid = bsxfun(@plus, hidden_presigmoid, permute(model.layers{l}.c, [2,3,4,5,1]));
        batch = 1 ./ (1 + exp(-hidden_presigmoid));
    else
        batch_size = size(batch,1);
        batch = reshape(batch, batch_size, []);
        hidden_presigmoid = bsxfun(@plus, ...
            batch * model.layers{l}.w, model.layers{l}.c);
        batch = 1 ./ ( 1 + exp(- hidden_presigmoid) );
    end
end


function mv=v_roteucode(m)
%V_ROTEUCODE decodes a string specifying a rotation axis sequence
%     M(n)     a string of n characters from the set determining the order of rotation axes
%              as listed below. Note that the control characters 'rdoOaA' may occur anywhere in the string:
%                'x','y','z'    rotate around the given axis by the corresponding angle
%                               given in e()
%                '1','2','3'    90 degree rotation around x,y or z axis; doesn't use a value from e()
%                '4','5','6'    180 degree rotation around x,y or z axis; doesn't use a value from e()
%                '7','8','9'    270 degree rotation around x,y or z axis; doesn't use a value from e()
%                'r','d'        all angles are given in radians or degrees  [radians]
%             'o','O','a','A'   selects whether to rotate the object or the coordinate axes and
%                               whether the rotation axes remain fixed in space for consecutive
%                               rotations (extrinsic) or else move with each rotation (intrinsic).
%                                  'o' = object-extrinsic [default]
%                                  'O' = object-intrinsic
%                                  'a' = axes-extrinsic
%                                  'A' = axes-intrinsic
% Outputs:
%
%     mv(7,k)    where k-1 is the number of non-control characters in the input string m
%                    mv(1,j) = Code for the j'th rotation: 1,2,3 for x,y,z rotation and 4 to 12 for the fixed rotations listed above.
%                              All entries are in the range [1,12] except for mv(1,k)=0.
%                    mv(2,j) = index into euler angle array for x,y,z rotations. mv(2,k) gives total number of euler angles needed
%                    mv(3,j) = rotation class before rotation j. mv(3,k) is the final rotation class and equals 52 for arbitrary rotations.
%                    mv(4,j) = index into a vectorized matrix of the entry that becomes non-zero after rotation j
%                    mv(5,j) = index into a vectorized matrix of the other changing element in the same column
%                    mv(6,j) = +-1 = sign of the sine term affecting entry mv(4,j). For mv(1,j) in [1,3], mv(6,j)=0 if the rotation is unnecessary
%                    mv(7,j) = +-1 = sign of entry mv(5,j) before rotation j if known
%                Special entries:
%                    mv(7,k) = -1 to invert the rotation (i.e. transpose the matrix) or +1 otherwise
%                    mv(4,k) = scale factor for euler angles: +-1 or +-pi/180
%
%
% The string M specifies the seqeunce of axes about which the rotations are performed. There are 12
% possible 3-character sequences that avoid consecutive repetitions. These are 'Euler angles' if
% there is a repeated axis or 'Tait-Bryan angles' if not. Common choices are:
% (1) 'zxz' the most common Euler angle set
% (2) 'xyz' corresponds to 'roll, pitch, yaw' for an aeroplane heading in the x direction with y to
%     the right and z down. The intrinsic equivalent is 'Ozyx' corresponding to 'yaw, pitch, roll'.
% (3) 'z1z1z' involves 5 rotations, in which all the non-fixed rotations are around the z axis.
%

%      Copyright (C) Mike Brookes 2007-2020
%      Version: $Id: v_roteucode.m 11260 2020-07-18 20:07:58Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

persistent mes trmap zel mch mvch  jch nch
if isempty(mes)     % setup fixed arrays and initialize cache of mode strings
    nch=5;          % size of cache
    mch=cell(nch,1); % cache of input character strings
    mvch=cell(nch,1);  % cache of output mv codes
    flefch=zeros(nch,2);  % cache of output flef codes
    jch=(1:nch); % cache usage order jch(1) is the most recent, jch(nch) the oldest
    for i=1:nch
        mch{i}='';
        mvch{i}=[0;0;1;1;0;0;1];
    end
    mes=[1:3 10:12 7:9 4:6]; % sign reversal look-up table
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % The trmap and zel arrays contain information about each of 52 different  %
    % patterns of -1,0,+1 that may exist in a rotation matrix as follows:      %
    %                                                                          %
    %   1-3 : identity matrix rows in order: 123, 231, 312                     %
    %   4-6 : negated identity matrix rows in order: 132, 213, 321             %
    %   7-12: As 1-6 but with rows 2,3 negated                                 %
    %  13-18: As 1-6 but with rows 1,3 negated                                 %
    %  19-24: As 1-6 but with rows 1,2 negated                                 %
    %  25-33: +1 in position (i-24) and 0's in remainder of this row and col   %
    %  34-42: -1 in position (i-24) and 0's in remainder of this row and col   %
    %  43-51: 0 in position (i-42)                                             %
    %  52: no special symmetry                                                 %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % trmap(i,j) gives the pattern that i is transformed into by rotation j    %
    % where j=1:3 corresponds to x,y,z and j=4:12 corresponds to the 9         %
    % multiples of 90 degree rotations listed in the main comments.            %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    trmap=[ 25 29 33 16 24 11  7 13 19 22 12 17;
        28 32 27 17 22 12  8 14 20 23 10 18;
        31 26 30 18 23 10  9 15 21 24 11 16;
        34 41 39 13 20  9 10 16 22 19  8 15;
        37 35 42 14 21  7 11 17 23 20  9 13;
        40 38 36 15 19  8 12 18 24 21  7 14;
        25 38 42 22  6 23  1 19 13 16 18  5;
        28 41 36 23  4 24  2 20 14 17 16  6;
        31 35 39 24  5 22  3 21 15 18 17  4;
        34 32 30 19  2 21  4 22 16 13 14  3;
        37 26 33 20  3 19  5 23 17 14 15  1;
        40 29 27 21  1 20  6 24 18 15 13  2;
        34 29 42 10 12  5 19  1  7  4 24 23;
        37 32 36 11 10  6 20  2  8  5 22 24;
        40 26 39 12 11  4 21  3  9  6 23 22;
        25 41 30  7  8  3 22  4 10  1 20 21;
        28 35 33  8  9  1 23  5 11  2 21 19;
        31 38 27  9  7  2 24  6 12  3 19 20;
        34 38 33  4 18 17 13  7  1 10  6 11;
        37 41 27  5 16 18 14  8  2 11  4 12;
        40 35 30  6 17 16 15  9  3 12  5 10;
        25 32 39  1 14 15 16 10  4  7  2  9;
        28 26 42  2 15 13 17 11  5  8  3  7;
        31 29 36  3 13 14 18 12  6  9  1  8;
        25 44 45 25 36 26 25 34 34 25 27 35;
        43 26 45 27 26 34 35 26 35 36 26 25;
        43 44 27 35 25 27 36 36 27 26 34 27;
        28 47 48 28 39 29 28 37 37 28 30 38;
        46 29 48 30 29 37 38 29 38 39 29 28;
        46 47 30 38 28 30 39 39 30 29 37 30;
        31 50 51 31 42 32 31 40 40 31 33 41;
        49 32 51 33 32 40 41 32 41 42 32 31;
        49 50 33 41 31 33 42 42 33 32 40 33;
        34 44 45 34 27 35 34 25 25 34 36 26;
        43 35 45 36 35 25 26 35 26 27 35 34;
        43 44 36 26 34 36 27 27 36 35 25 36;
        37 47 48 37 30 38 37 28 28 37 39 29;
        46 38 48 39 38 28 29 38 29 30 38 37;
        46 47 39 29 37 39 30 30 39 38 28 39;
        40 50 51 40 33 41 40 31 31 40 42 32;
        49 41 51 42 41 31 32 41 32 33 41 40;
        49 50 42 32 40 42 33 33 42 41 31 42;
        43 52 52 43 45 44 43 43 43 43 45 44;
        52 44 52 45 44 43 44 44 44 45 44 43;
        52 52 45 44 43 45 45 45 45 44 43 45;
        46 52 52 46 48 47 46 46 46 46 48 47;
        52 47 52 48 47 46 47 47 47 48 47 46;
        52 52 48 47 46 48 48 48 48 47 46 48;
        49 52 52 49 51 50 49 49 49 49 51 50;
        52 50 52 51 50 49 50 50 50 51 50 49;
        52 52 51 50 49 51 51 51 51 50 49 51;
        52 52 52 52 52 52 52 52 52 52 52 52];
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Each Euler angle is chosen so that the inverse rotation forces a specific element  %
    % of the rotation matrix to zero. zel(k,j,i) gives information about which element   %
    % ceases to be zero when a rotation around axis j is applied to pattern i.           %
    %    k=1 gives the index into a vectorized matrix of the entry that becomes non-zero %
    %    k=2 gives the index of the other element in the same column that changes        %
    %    k=3 gives the sign of the sine term affecting the first of these entries        %
    %    k=4 gives the sign of the initial value of the second of these entries if known %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    zel=reshape([  6  5  1  1  3  1 -1  1  2  1  1  1;
        2  3 -1  1  1  3  1  1  5  4  1  1;
        3  2  1  1  4  6  1  1  1  2 -1  1;
        5  6 -1 -1  3  1 -1 -1  2  1  1 -1;
        3  2  1 -1  6  4 -1 -1  1  2 -1 -1;
        2  3 -1 -1  1  3  1 -1  4  5 -1 -1;
        6  5  1 -1  3  1 -1  1  2  1  1  1;
        2  3 -1 -1  1  3  1 -1  5  4  1  1;
        3  2  1 -1  4  6  1 -1  1  2 -1 -1;
        5  6 -1  1  3  1 -1 -1  2  1  1 -1;
        3  2  1  1  6  4 -1 -1  1  2 -1  1;
        2  3 -1  1  1  3  1  1  4  5 -1  1;
        6  5  1  1  3  1 -1 -1  2  1  1 -1;
        2  3 -1 -1  1  3  1 -1  5  4  1 -1;
        3  2  1  1  4  6  1 -1  1  2 -1  1;
        5  6 -1  1  3  1 -1  1  2  1  1  1;
        3  2  1 -1  6  4 -1  1  1  2 -1 -1;
        2  3 -1  1  1  3  1  1  4  5 -1 -1;
        6  5  1 -1  3  1 -1 -1  2  1  1 -1;
        2  3 -1  1  1  3  1  1  5  4  1 -1;
        3  2  1 -1  4  6  1  1  1  2 -1 -1;
        5  6 -1 -1  3  1 -1  1  2  1  1  1;
        3  2  1  1  6  4 -1  1  1  2 -1  1;
        2  3 -1 -1  1  3  1 -1  4  5 -1  1;
        0  0  0  0  3  1 -1  1  2  1  1  1;
        3  2  1  1  0  0  0  0  1  2 -1  1;
        2  3 -1  1  1  3  1  1  0  0  0  0;
        0  0  0  0  6  4 -1  1  5  4  1  1;
        6  5  1  1  0  0  0  0  4  5 -1  1;
        5  6 -1  1  4  6  1  1  0  0  0  0;
        0  0  0  0  9  7 -1  1  8  7  1  1;
        9  8  1  1  0  0  0  0  7  8 -1  1;
        8  9 -1  1  7  9  1  1  0  0  0  0;
        0  0  0  0  3  1 -1 -1  2  1  1 -1;
        3  2  1 -1  0  0  0  0  1  2 -1 -1;
        2  3 -1 -1  1  3  1 -1  0  0  0  0;
        0  0  0  0  6  4 -1 -1  5  4  1 -1;
        6  5  1 -1  0  0  0  0  4  5 -1 -1;
        5  6 -1 -1  4  6  1 -1  0  0  0  0;
        0  0  0  0  9  7 -1 -1  8  7  1 -1;
        9  8  1 -1  0  0  0  0  7  8 -1 -1;
        8  9 -1 -1  7  9  1 -1  0  0  0  0;
        0  0  0  0  1  3  1  1  1  2 -1  1;
        2  3 -1  1  0  0  0  0  2  1  1  1;
        3  2  1  1  3  1 -1  1  0  0  0  0;
        0  0  0  0  4  6  1  1  4  5 -1  1;
        5  6 -1  1  0  0  0  0  5  4  1  1;
        6  5  1  1  6  4 -1  1  0  0  0  0;
        0  0  0  0  7  9  1  1  7  8 -1  1;
        8  9 -1  1  0  0  0  0  8  7  1  1;
        9  8  1  1  9  7 -1  1  0  0  0  0;
        0  0  0  0  0  0  0  0  0  0  0  0]',4,3,52);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Convert the m string
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if ~ischar(m) % lecacy call with integer m argument
    m=char(m+'w'); % convert to characters
end
ich=find(strcmp(m,mch),1);      % check if already in the cache
if isempty(ich)                 % not yet in the cache
    mm=m-'w';                   % convert to integers with x -> 1
    mi=mm>=-31 & mm<=-29;       % find characters XYZ
    mm(mi)=mm(mi)+32;           % convert XYZ to xyz (for compatibility)
    mi=mm>=-70 & mm<=-62;       % find digits 1:9
    mm(mi)=mm(mi)+74;           % convert to 4:12
    mi=mm<=0;                   % select control characters
    mc=mm(mi);                  % controls
    mm=mm(~mi);                 % rotations
    ef=1;                       % angle scale factor
    es=1;                       % angle sign
    fl=1;                       % default to no rotation matrix tranposing
    for i=1:length(mc)
        switch mc(i)
            case -5             % 'r' = radians
            case -19            % 'd' = degrees
                ef=pi/180;      % scale factor to convert to radians
            case -37            % 'R' = negated radians
                ef=-1;
            case -51            % 'D' = negated degrees
                ef=-pi/180;      % scale factor to convert to radians
            case -8             % 'o' = object-extrinsic
            case -40            % 'O' = object-intrinsic
                fl=-1;
                es=-1;
            case -22            % 'a' = axes-extrinsic
                fl=-1;
            case -54            % 'A' = axes-intrinsic
                es=-1;
            otherwise
                error('Invalid character: %s',mc(i)+'w')
        end
    end
    ef=ef*es;               % change sign of scale factor if necessary
    if es<0
        mm=mes(mm);         % sign-reverse: interchage 4,5,6 with 10,11,12
    end
    nm=length(mm);
    mv=zeros(7,nm+1);
    mv(1,:)=[mm 0];
    mv(2,:)=cumsum([mm<=3 0]);      % index into euler angle array
    mv(3,1)=1; % initial pattern is the identity matrix
    for i=1:nm                % loop for each rotation
        mmi=mm(i); % rotation code
        mv(3,i+1)=trmap(mv(3,i),mmi); % pattern ID after rotation
        if mmi<4
            mv(4:7,i)=zel(:,mmi,mv(3,i)); % information about which matrix elements change from zero
        end
    end
    mv(end)=fl;
    mv(end-3)=ef;
    % now save in the cache
    ich=jch(nch);       % find oldest cache entry
    mch{ich}=m;                 % save input string
    mvch{ich}=mv;               % save parameters
    jch=[ich jch(1:nch-1)];     % age all the other cache entries
else                            % already in the cache
    kch=find(jch==ich,1);       % find existing ich entry
    jch(1:kch)=[ich jch(1:kch-1)];
    mv=mvch{ich};               % retrieve from cache
end


function C = train_HLDA(XTr, YTr, nSegments, varargin)
% TRAIN_HLDA - Hierarchical linear discriminant analysis
%
%Synopsis:
%   C = train_HLDA(XTr, YTr, nSegments, varargin)
%
%Arguments:
%   XTR: DOUBLE [TxNxM] - Data matrix, with T temporal features, N
%                           N spatial features, and M
%                           training points/examples.
%   YTR: INT [CxM]      - Class membership labels of points in X_TR. C by M
%                           matrix of training labels, with C representing
%                           the number of classes and M the number of
%                           training examples/points.
%                           YTR(i,j)==1 if the point j belongs to class i.
%   nSegments: INT      - the number of non-overlapping segments the interval
%                           should be separated in
%   OPT: PROPLIST       - Structure or property/value list of optional
%                           properties. Options are also passed to clsutil_shrinkage.
%     'Regression'  - BOOL (default 0): If true, the top level classifier is
%                           a logistic regression classifier.
%     'nChannels'   - INT (default 0): Used when using 'crossvalidation' in order to
%                           reconstruct the 2D feature matrix to its original 3D
%                           [T x Ch x N] shape.
%Returns:
%   C: STRUCT           - Structure containing the trained classifiers for
%                           each segment and the final top level
%                           classifier. The structure C includes the fields:
%       'seg': STRUCT []      - contains a trained LDA classifier for each segment
%       'final': STRUCT       - the final top-level LDA classifier
%       'nChannels': INT      - (optional) the number of channels in the training data
%Description:
%   train_HLDA trains a hierarchical LDA classifier given training data,
%   labels and a number of segments. Either LDA (default) or logistic regression
%   is used as a top-level classifier.
%
%   References:Gerson, A.D., Parra, L.C., Sajda, P.: Cortically coupled
%   computer vision for rapid image search. IEEE Transactions on Neural
%   Systems and Rehabilitation Engineering 14, 174–179 (2006).

%
%
%Examples:
%   train_HLDA(XTr, YTr, nSegments)
%   train_HLDA(XTr, YTr, nSegments, 'Regression', 1, 'nChannels', 64)
%
%See also:
%   APPLY_HLDA

props= {'Regression'      0                             'BOOL'
        'nChannels'       0                             'INT'
       };

opt= opt_proplistToStruct(varargin{:});
[opt, isdefault]= opt_setDefaults(opt, props);
opt_checkProplist(opt, props);

% validate argument types
misc_checkType(XTr, 'DOUBLE');

if opt.nChannels ~= 0
  misc_checkType(XTr, 'DOUBLE[- -]');
end

misc_checkType(YTr, 'DOUBLE[2 -]');
misc_checkType(nSegments, 'INT');

dims = size(XTr);

% make data matrix 3D if it has been tranformed to 2D for crossvalidation
if (length(size(XTr)) == 2) && opt.nChannels > 0
  XTr = reshape(XTr, [], opt.nChannels, dims(2));
end

dims = size(XTr);

%boundary indices between segments
seg_idx = round(linspace(0, dims(1), nSegments+1));

seg_scores = zeros(nSegments, dims(end));

for i = 1:nSegments
    if length(size(XTr)) > 2
      seg = XTr(seg_idx(i) + 1 : seg_idx(i + 1), :, :); % i-th segment of X
      seg = reshape(seg, [], dims(end));
    else
      seg = XTr(seg_idx(i) + 1 : seg_idx(i + 1), :); % i-th segment of X
    end
    %train LDA classifier for this segment
    seg_LDA = train_RLDAshrink(seg, YTr, 'Scaling', 1);
    C.seg(i) = seg_LDA;
    seg_scores(i,:) = apply_separatingHyperplane(seg_LDA, seg); %get classifier scores for segment
end

if opt.Regression
    %logistic regression as top-level classifier
    C.final.B = mnrfit(seg_scores', YTr');
else
    %LDA as top-level classifier
    C.final = train_RLDAshrink(seg_scores, YTr, 'Scaling', 1);
end

% add number of channels to classifier so they can be used in the apply function
if opt.nChannels > 0
  C.nChannels = opt.nChannels;
end


% Mseq Toolbox
% Vesion 1.0 10-28-2001
% written by Giedrius Buracas,
% SNL-B, Salk Institute
%
%
% Purpose:
%
%  Allows to evaluate estimation efficiency of m-sequence and
%  rando-sequence based experimental designs for event-related
%  fMRI experiments
%
% Sequence generation
%
%  balancedRnd		Generates random sequences, with either
%			overlaping or non-overlaping events
%			Equal numbers of events of each type is assumed
%
%  mseq			Generates binary, ternary, or five level
%			m-sequences. A unique feature of this code is that
%			many m-sequences of given parameters
%			can be generated
%
%  cycorr		generates a cyclical autocorrelation
%			function of a given sequence. This is
%			the fastest way to test whether a given sequence
%			is an m-sequence
%
%  m2bin		converts a binary m-sequence [-1,1] to
%			a sequence of [0,1]
%
%  bin2m		converts a sequence of [0,1] to [-1,1]
%
% Calculation of estimation efficiency
%
%  makeEventMtrx	generates an event matrix from a matrix of
%			[0,1] whose each column gives timing for
%			each event type
%
%  efficiencyOFexpDesigns2 script that compares estimation
%			efficiency of random and m-seuence-based
%			experimental designs. In this case
%			simultaneously occuring events are permitted
%			(overlaping events)
%
%  effOFexpDesignsNoOverlap script that compares estimation
%			efficiency of random and m-seuence-based
%			experimental designs. In this case
%			simultaneously occuring events are not permitted
%			(non-overlaping events)
%
%
%  efficiencyOFexpDesignsCorrMtrx script that compares estimation
%			efficiency of random and m-seuence-based
%			experimental designs.
%			Efficiency is calculated with and without
%			fMRI noise
%
%
%
%


function a = svm(hyper)
%=============================================================================
% SVM Support Vector Machine object
%=============================================================================
% a=svm(hyperParam)
%
% Generates a svm object with given hyperparameters.
%
%
%   Hyperparameters (with defaults)
%   child=kernel         -- the kernel is stored as a member called "child"
%   C=Inf                -- the soft margin C parameter
%   ridge=1e-13          -- a ridge on the kernel
%   balanced_ridge=0     -- for unbalanced data
%   nu = 0               -- Schoelkopf's nu svm parameter
%   optimizer='default'  -- other choices={andre,quadprog,svmlight,
%                                          libsvm,svmtorch(linux only)}
%                           For "libsvm" optimizer you can specify the used cache size
%                           by the global variable "libsvm_cachesize".
%   alpha_cutoff=-1;     -- keep alphas with abs(a_i)>alpha_cutoff
%                           default keeps all alphas, another
%                           reasonable choice is e.g alpha_cutoff=1e-5 to remove
%                           zero alphas (i.e non-SVs) to speed up computations.
%
%   Model
%    alpha               -- the weights
%    b0                  -- the threshold
%    Xsv                 -- the Support Vectors
%
% Methods:
%  train, test, get_w
%
% Example:
%
%  d=gen(spiral({'m=200','n=2','noise=0.35'}));
%  [r,a]=train(cv(svm({kernel('rbf',1),'optimizer="andre"'})),d)
%  plot(a{1})
%
%=============================================================================
% Reference : A Tutorial on Support Vector Machines for Pattern Recognition
% Author    : Christopher J. C. Burges
% Link      : http://citeseer.ist.psu.edu/burges98tutorial.html
%=============================================================================

  %<<------hyperparam initialisation------------->>
  a.child=kernel;
  a.C=Inf;
  a.ridge=1e-13;
  a.balanced_ridge=0;
  a.nu = 0;
  a.optimizer='default';
  a.alpha_cutoff=-1;

  % <<-------------model----------------->>
  a.alpha=[];
  a.b0=0;
  a.Xsv=[];
  a.nob=0;

  algoType=algorithm('svm');
  a= class(a,'svm',algoType);

  a.algorithm.alias={'kern','child'}; % kernel aliases

 if nargin==1,
    eval_hyper;
 end;


function check = extreme_values_check ( a, b )

%*****************************************************************************80
%
%% EXTREME_VALUES_CHECK checks the parameters of the Extreme Values CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the parameters of the PDF.
%    0.0 < B.
%
%    Output, logical CHECK, is true if the parameters are legal.
%
  if ( b <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'EXTREME_VALUES_CHECK - Fatal error!\n' );
    fprintf ( 1, '  B <= 0.\n' );
    check = 0;
    return
  end

  check = 1;

  return
end


function pass = test_feval(pref)
% Test CHEBFUN3/FEVAL

if ( nargin == 0)
    pref = chebfunpref;
end
tol = 100*pref.cheb3Prefs.chebfun3eps;

seedRNG(42);

f = chebfun3t(@(x,y,z) x, [-1 2 -pi/2 pi -3 1]);
pass(1) = abs(f(0,0,0)) < tol*f.vscale;

pass(2) = abs(f(pi/6,pi/12,-1)-pi/6) < tol*f.vscale;

f = chebfun3t(@(x,y,z) y, [-1 2 -pi/2 pi -3 1]);
pass(3) = abs(f(0,0,0)) < tol;

pass(4) = abs(f(pi/6,pi/12,-1)-pi/12) < tol*f.vscale;

f = chebfun3t(@(x,y,z) z, [-1 2 -pi/2 pi -3 1]);
pass(5) = abs(f(0,0,0)) < tol;

pass(6) = abs(f(pi/6,pi/12,-1)+1) < tol*f.vscale;

% some harder tests.
f = @(x,y,z) cos(x) + sin(x.*y) + sin(z.*x);
g = chebfun3t(f);
pts = 2*rand(3,1) - 1;
pass(7) = abs(f(pts(1),pts(2),pts(3)) - g(pts(1),pts(2),pts(3))) < ...
    tol*g.vscale;

% Are we evaluating on arrays correctly?
r = rand(10,1);
s = rand(10,1);
t = rand(10,1);
[rr, ss, tt]=meshgrid(r,s,t);
pass(8) = max(abs((f(r,s,t) - g(r,s,t)))) < tol*g.vscale;

pass(9) = max(max(max(abs(f(rr,ss,tt) - g(rr,ss,tt))))) < tol*g.vscale;

% Does this work off [-1,1]^2
g = chebfun3t(f,[-pi/6 pi/2 -pi/12 sqrt(3) -3 1]); % strange domain.
r = 0.126986816293506; s = 0.632359246225410; t = 0.351283361405006;
% three fixed random number in domain.
pass(10) = abs(f(r,s,t) - g(r,s,t))<tol*g.vscale;

% Are we evaluating on arrays correctly
pass(11) = abs(f(r,s,t) - g(r,s,t)) < tol*g.vscale;

pass(12) = max(max(max(abs(f(rr,ss,tt) - g(rr,ss,tt))))) < tol*g.vscale;

% vector inputs
ff = @(x,y,z) sin(pi*(x+y+z));
f = chebfun3t(ff);
xx = linspace(-1, 1, 100)';
yy = linspace(-1, 1, 100)';
zz = linspace(-1, 1, 100)';
F = f(xx,yy,zz);
FF = ff(xx,yy,zz);
pass(13) = norm(F(:) - FF(:)) < 100*tol;

% random vector inputs
ff = @(x,y,z) sin(pi*(x+y+z));
f = chebfun3t(ff);
xx = rand(100, 1);
yy = rand(100, 1);
zz = rand(100, 1);
F = f(xx,yy,zz);
FF = ff(xx,yy,zz);
pass(14) = norm(F - FF) < 100*tol;

% meshgrid
ff = @(x,y,z) sin(pi*(x+y+z));
f = chebfun3t(ff);
[xx, yy, zz] = meshgrid(linspace(-1, 1, 100));
F = f(xx,yy,zz);
FF = ff(xx,yy,zz);
pass(15) = norm(F(:) - FF(:)) < 100*tol;

% ndgrid
ff = @(x,y,z) sin(pi*(x+y+z));
f = chebfun3t(ff);
[xx, yy, zz] = ndgrid(linspace(-1, 1, 100));
F = f(xx,yy,zz);
FF = ff(xx,yy,zz);
pass(16) = norm(F(:) - FF(:)) < 100*tol;

% random tensor inputs
ff = @(x,y,z) sin(pi*(x+y+z));
f = chebfun3t(ff);
xx = rand(10, 20, 30);
yy = rand(10, 20, 30);
zz = rand(10, 20, 30);
F = f(xx,yy,zz);
FF = ff(xx,yy,zz);
pass(17) = norm(F(:) - FF(:)) < 100*tol;

end


%%******************************************************************
%% randlowranksdp.m : creates random feasible SDP problems where the
%%                constraint matrices are low-rank matrices of the
%%                form V*diag(d)*V'.
%%
%% [blk2,At2,C,b,blk,At] = randlowranksdp(n,m1,m2,r);
%%
%% blk2,At2: data with low-rank structure coded.
%% blk, At:  data without taking low-rank structure into account.
%%
%% n = size of the sdp variable
%% m1 = number of general constraints
%% m2 = number of low-rank constraints
%% r = rank of each constraint matrix.
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

  function [blk2,At2,C,b,blk,At] = randlowranksdp(n,m1,m2,r)

  blk = cell(1,2);
  if (m1 > 0)
     [blk,At0,C,b0] = randsdp(n,[],[],m1);
  else
     b0 = []; At0 = cell(1);
  end
  if (m2 > 0)
     if (nargout > 4)
        [blk2,At2,C,b2,blk,At1] = randlowranksdpfun(n,m2,r);
     else
        [blk2,At2,C,b2] = randlowranksdpfun(n,m2,r);
     end
  else
     b2 = []; At1 = cell(1); blk2 = blk;
  end
  b = [b0; b2];
  if (nargout > 4)
     At{1} = [At0{1}, At1{1}];
  end
  At2{1,1} = At0{1};
%%******************************************************************
%%******************************************************************
  function [blk2,At2,C,b,blk,At] = randlowranksdpfun(n,m,r)

  randn('state',0);

  blk{1,1} = 's'; blk{1,2} = n;
  blk2{1,1} = 's'; blk2{1,2} = n; blk2{1,3} = r*ones(1,m);
  %%
  %% construct data with low rank structure
  %%
  At2 = cell(1,3);
  b = zeros(m,1);
  X0 = randn(n); X0 = X0*X0'; X0 = 0.5*(X0+X0');
  ss = [0,cumsum(blk2{1,3})];
  V = randn(n,m*r);
  dd = [];
  for k = 1:length(blk2{1,3})
     idx = [ss(k)+1 : ss(k+1)];
     len = blk2{1,3}(k);
     Dk = randn(len,len); Dk = 0.5*(Dk+Dk');
     [ii,jj,vv] = find(Dk);
     numnz = length(ii);
     dd = [dd; k*ones(numnz,1),ii,jj,vv]; %% each row has the form [constr,i,j,val]
     tmp1 = X0*V(:,idx);
     tmp2 = V(:,idx)*Dk;
     b(k) = sum(sum(tmp1.*tmp2));
  end
  At2{1,2} = V; At2{1,3} = dd;
  Z0 = X0;
  y0 = randn(m,1);
  Aty = At2{1,2}*spdiags(mexexpand(blk2{1,3},y0),0,r*m,r*m)*At2{1,2}';
  C{1} = Z0 + norm(Aty,'fro')*speye(n,n) + Aty;
  C{1} = 0.5*(C{1}+C{1}');
  %%
  %% construct data without exploiting low rank structure
  %%
  if (nargout > 4)
     idxD = [0; find(diff(dd(:,1))); size(dd,1)];
     for k = 1:m
        idx = [ss(k)+1 : ss(k+1)];
        Vk = At2{1,2}(:,idx);
        len = blk2{1,3}(k);
        idx2 = [idxD(k)+1:idxD(k+1)];
        Dk = spconvert([dd(idx2,2:4); len,len,0]);
        A{k} = Vk*Dk*Vk';
     end
     At = svec(blk,A);
  end
%%******************************************************************


function [w, infos] = hb(problem, in_options)
% Heavy Ball algorithm.
%
% Inputs:
%       problem     function (cost/grad/hess)
%       in_options  options
% Output:
%       w           solution of w
%       infos       information
%
% This file is part of GDLibrary and SGDLibrary.
%
% Created by H.Kasai on Oct. 28, 2020

    % set dimensions and samples
    d = problem.dim;
    n = problem.samples;

    % set local options
    local_options = [];
    local_options.step_alg = 'backtracking';
    local_options.beta = 0.01;

    % merge options
    options = mergeOptions(get_default_options(d), local_options);
    options = mergeOptions(options, in_options);

    % initialise
    iter = 0;
    grad_calc_count = 0;
    w = options.w_init;
    w_old = w;
    prev_step = options.step_init;

    % for stepsize
    if strcmp(options.step_alg, 'fix') || strcmp(options.step_alg, 'no_change')
        if isprop(problem, 'L')
            if problem.L > 0
                if isprop(problem, 'mu')
                    if problem.mu > 0
                        % This casse is L-smooth and mu-strongly convex.
                        cn = problem.L/problem.mu;
                        options.step_init = 4/(sqrt(problem.L)+sqrt(problem.mu))^2;
                        options.beta = ( (sqrt(cn)-1)/(sqrt(cn)+1) )^2;
                    else
                        % This casse is L-smooth
                        options.step_init = 1/problem.L;
                    end
                else
                    % This casse is L-smooth
                    options.step_init = 1/problem.L;
                end
            else
                options.step_alg = 'backtracking';
            end
        end
    end

    % initialize by BB step-size
    if strcmp(options.step_init_alg, 'bb_init')
        options.step_init = bb_init(problem, w);
    end

    % store first infos
    clear infos;
    [infos, f_val, optgap, grad, gnorm] = store_infos(problem, w, options, [], iter, grad_calc_count, 0);
    grad_old = grad;

    % display info
    if options.verbose
        fprintf('HB: Iter = %03d, cost = %.24e, gnorm = %.4e, optgap = %.4e\n', iter, f_val, gnorm, optgap);
    end

    % set start time
    start_time = tic();

    % main loop
    while (optgap > options.tol_optgap) && (gnorm > options.tol_gnorm) && (iter < options.max_epoch)

        options.iter = iter;
        [step, ~] = options.linesearchfun(options.step_alg, problem, w, w_old, grad, grad_old, prev_step, options);

        prev_step = step;
        w_w_old_diff = w - w_old;
        w_old = w;

        % update w
        w = w - step * grad + options.beta * w_w_old_diff;

        % proximal operator
        if ismethod(problem, 'prox')
            w = problem.prox(w, step);
        end

        % store gradient
        grad_old = grad;

        % measure elapsed time
        elapsed_time = toc(start_time);

        % count gradient evaluations
        grad_calc_count = grad_calc_count + n;

        % update iter
        iter = iter + 1;

        % store infos
        [infos, f_val, optgap, grad, gnorm] = store_infos(problem, w, options, infos, iter, grad_calc_count, elapsed_time);

        % display infos
        if options.verbose
            fprintf('HB: Iter = %03d, cost = %.24e, gnorm = %.4e, optgap = %.4e\n', iter, f_val, gnorm, optgap);
        end
    end

    if gnorm < options.tol_gnorm
        fprintf('Gradient norm tolerance reached: tol_gnorm = %g\n', options.tol_gnorm);
    elseif optgap < options.tol_optgap
        fprintf('Optimality gap tolerance reached: tol_optgap = %g\n', options.tol_optgap);
    elseif iter == options.max_epoch
        fprintf('Max iter reached: max_epoch = %g\n', options.max_epoch);
    end

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_PA_10_6DOF(robot, T)
%   Solves the inverse kinematic problem for the MITSUBISHI PA-10 robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC_PA_10_6DOF returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   robot=load_robot('MISUBISHI', 'pa-10');
%   q = [0 0 0 0 0 0];
%   T = directkinematic(robot, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(robot, T);
%   check that all of them are feasible solutions!
%   and every Ti equals T
%   for i=1:8,
%        Ti = directkinematic( robot, qinv(:,i))
%   end
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function q = inversekinematic_pa_10_6DOF(robot, T)

%initialize, eight possible solutions
q=zeros(6,8);

%theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
%alpha = eval(robot.DH.alpha);

%L1=d(1);
%L2=a(2);
%L3=d(4);
L6=d(6);

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Calculamos la posici�n de la mu�eca. W es el tercer vector en el extremo
%del brazo
W = T(1:3,3);
%Pm: posici�n de la mu�eca
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
q1=atan2(Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);

q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);

q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%leave only the real part of the solutions
q=real(q);

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));
q(3,:) = normalize(q(3,:));

% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    qtemp = solve_spherical_wrist_pa_10(robot, q(:,i), T, 1); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solve_spherical_wrist_pa_10(robot, q(:,i), T, -1); %wrist down
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry
L2=a(2);
L3=d(4);

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(p1(2), p1(1));
gamma = (acos((L2^2+r^2-L3^2)/(2*r*L2)));
if ~isreal(gamma)
    disp('WARNING:inversekinematic_pa_10: the point is not reachable for this configuration, imaginary solutions');
    %gamma = real(gamma);
end
%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3
q2(1) = beta + gamma-pi/2; %elbow up
q2(2) = beta - gamma-pi/2; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
%theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
%alpha = eval(robot.DH.alpha);

%See geometry
L2=a(2);
L3=d(4);

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

eta = acos((L2^2 + L3^2 - r^2)/(2*L2*L3));

if ~isreal(eta)
   disp('WARNING:inversekinematic_pa_10: the point is not reachable for this configuration, imaginary solutions');
   %eta = real(eta);
end
%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = -pi + eta;
q3(2) = -eta + pi;

% Solve the special case of this spherical wrist
% For wrists that whose reference systems have been placed as in the
% ABB IRB 140--> use solve_spherical_wrist2
% For wrists with the same orientation as in the KUKA KR30_jet
%--> use solve_spherical_wrist
function q = solve_spherical_wrist_pa_10(robot, q, T, wrist)

% T is the noa matrix defining the position/orientation of the end
% effector's reference system
vx6=T(1:3,1);
vz5=T(1:3,3); % The vector a z6=T(1:3,3) is coincident with z5

% Obtain the position and orientation of the system 3
% using the already computed joints q1, q2 and q3
T01=dh(robot, q, 1);
T12=dh(robot, q, 2);
T23=dh(robot, q, 3);
T03=T01*T12*T23;

vx3=T03(1:3,1);
vy3=T03(1:3,2);
vz3=T03(1:3,3);

% find z4 normal to the plane formed by z3 and a
vz4=cross(vz3, vz5);	% end effector's vector a: T(1:3,3)

% in case of degenerate solution,
% when vz3 and vz6 are parallel--> then z4=0 0 0, choose q(4)=0 as solution
if norm(vz4) <= 0.00000001
    if wrist == 1 %wrist up
        q(4)=0;
    else
        q(4)=-pi; %wrist down
    end
else
    %this is the normal and most frequent solution
    cosq4=wrist*dot(vy3,vz4);
    sinq4=wrist*dot(-vx3,vz4);
    q(4)=atan2(sinq4, cosq4);
end
%propagate the value of q(4) to compute the system 4
T34=dh(robot, q, 4);
T04=T03*T34;
vx4=T04(1:3,1);
vy4=T04(1:3,2);

% solve for q5
cosq5=dot(-vy4,vz5);
sinq5=dot(vx4,vz5);
q(5)=atan2(sinq5, cosq5);

%propagate now q(5) to compute T05
T45=dh(robot, q, 5);
T05=T04*T45;
vx5=T05(1:3,1);
vy5=T05(1:3,2);

% solve for q6
cosq6=dot(vx6,vx5);
sinq6=dot(vx6,vy5);
q(6)=atan2(sinq6, cosq6);


classdef OFA < ALGORITHM
% <single> <real/integer> <large/none> <constrained/none>
% Optimal foraging algorithm

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

    methods
        function main(Algorithm,Problem)
            %% Generate random population
            Population = Problem.Initialization();
            [~,rank]   = sort(FitnessSingle(Population));
            Population = Population(rank);

            %% Optimization
            while Algorithm.NotTerminated(Population)
                PopDec     = Population.decs;
                OffDec     = PopDec + Problem.FE./Problem.maxFE.*(rand(size(PopDec))-rand(size(PopDec))).*(PopDec-PopDec([end,floor(unifrnd(ones(1,end-1),2:end))],:));
                Offspring  = Problem.Evaluation(OffDec);
                lambda     = rand(Problem.N,1);
                replace    = lambda.*FitnessSingle(Offspring)./(1+lambda*ceil(Problem.FE/Problem.N)) < FitnessSingle(Population)/ceil(Problem.FE/Problem.N);
                Population(replace) = Offspring(replace);
                [~,rank]   = sort(FitnessSingle(Population));
                Population = Population(rank);
            end
        end
    end
end