function w = perform_vf_reorientation(v, options)

% perform_vf_reorientation - try to reorient the vf.
%
%   w = perform_vf_reorientation(v, options);
%
%   Flip the orientation of some vectors to ensure a coherent vector field.
%   v and w are (n,n,2) 2D vector field matrices.
%
%   options.method can be 'xproj', 'yproj', 'circproj', 'localproj',
%   'custproj', 'randomized', 'laplacian'.
%
%   'randomized' uses a slow simulated annealing method to optimize
%   an Ising energy.
%   'laplacian' solves a big linear system and is the best method (but slow).
%
%   For 'custproj' you have to provide an additional vector options.w.
%
%   See also: perform_vf_normalization.
%
%   Copyright (c) 2007 Gabriel Peyre

options.null = 0;
w = getoptions(options, 'w', [1 0]);
method = getoptions(options, 'method', 'xproj');

if size(v,3)~=2
    error('Works only for 2D vector fields.');
end

n = size(v,1);
p = size(v,2);

if strcmp(method, 'laplacian')

    % Try to solve the following linear system for some s(k) at each
    % location k:
    %   s(k) = 1/|V_k| * sum_{i \in V_k} s(k)*cos(theta(k)-theta(i))
    % where V_k are the neighbors of point k and theta(k) is the angle
    % theta(k)=atan2(v_y(k),v_x(k));

    % normalize
    d = sqrt(sum(v.^2,3)); d(d<eps) = 1;
    v = v ./ repmat(d,[1 1 2]);

    vx = v(:,:,1); vy = v(:,:,2);
    theta = atan2(vy,vx);

    i = []; j = []; s = [];
    sdiag = zeros(n^2,1);   % weights along the diagonal
    [Y,X] = meshgrid(1:n,1:n);
    I = X(:)+(Y(:)-1)*n;

    deplx = [1 -1 0 0];
    deply = [0 0 1 -1];
    for k=1:length(deplx)
        dx = deplx(k); dy = deply(k);
        J = find( X+dx<=n & X+dx>0 & Y+dy<=n & Y+dy>0);
        I1 = I(J);
        J1 = X(J)+dx+(Y(J)+dy-1)*n;
        i = [i; I1]; j = [j; J1];
        s = [s; cos( theta(I1)-theta(J1) )];
        sdiag(J) = sdiag(J)+1;
    end
    % add diagonal terms
    i = [i; I]; j = [j; I]; s = [s; -sdiag];
    % enforce constraint
    I = find(i>1);
    i = i(I); j = j(I); s = s(I);
    i = [i; 1]; j = [j; 1]; s = [s; 1];
    L = sparse(i,j,s);
    % solve
    y = zeros(n^2,1); y(1)=1;
    use_cg = getoptions(options, 'use_cg', 0);
    % use_cg = 0;
    if use_cg
        if not(isfield(options, 'niter_max'))
            options.niter_max = 500;
        end
        options.x = y*0+1;
        [s,err] = perform_conjugate_gradient(L'*L,L'*y,options);
    else
        s = L\y;
    end

    s = reshape(s,n,n);
    s = sign(s);
    w = v .* repmat(d, [1 1 2]) .* repmat(s, [1 1 2]);

    return;
end

if strcmp(method, 'laplacian1')
    %%%%%% OLD %%%%%%%%%
    % normalize
    d = sqrt(sum(v.^2,3)); d(d<eps) = 1;
    v = v ./ repmat(d,[1 1 2]);
    % padd with zeros
    v0 = zeros(n+2,n+2,2);
    v0(2:end-1,2:end-1,:) = v; v = v0;

    [dY,dX] = meshgrid(-1:1, -1:1); dX = dX(:); dY = dY(:);
    dX((end+1)/2) = []; dY((end+1)/2) = [];

    i = [n^2+1]; j = [n/2+(n/2-1)*n]; z = [1];
    u = zeros(n^2,1); % diagonal term
    for t=1:length(dX)
        dx = dX(t); dy = dY(t);
        % interaction
        a = sum( v(2:end-1,2:end-1,:) .* v(2+dx:end-1+dx,2+dy:end-1+dy,:), 3);
        [y,x] = meshgrid(1:n,1:n);
        I = find(x+dx>=1 & x+dx<=n & y+dy>=1 & y+dy<=n);
        x = x(I); y = y(I);
        w = a( max(1-dx,1):min(end-dx,end), max(1-dy,1):min(end-dy,end) );
        w(abs(w)<1e-3) = 1e-3;
        i = [i; x+(y-1)*n ];
        j = [j; x+dx+(y+dy-1)*n ];
        z = [ z; w(:) ];
        % add to diagonal
        u(x+(y-1)*n) = u(x+(y-1)*n) + 1;
    end
    % add diagonal term
    [y,x] = meshgrid(1:n,1:n); x = x(:); y = y(:);
    i = [i; x+(y-1)*n ];
    j = [j; x+(y-1)*n ];
    z = [ z; -u(:) ];

    A = sparse(i,j,z);
    b = zeros(n^2+1, 1); b(end)=1;
    use_cg = getoptions(options, 'use_cg', 1);
    if use_cg
        if not(isfield(options, 'niter_max'))
            options.niter_max = 500;
        end
        y = b;
        A1 = A'*A; y = A'*b;
        options.x = y*0+1;
        [s,err] = perform_conjugate_gradient(A1,y,options);
    else
        s = A\b;
    end
    s = reshape(s,n,n);
    s = sign(s);

    w = v(2:end-1,2:end-1,:);
    w = w .* repmat(d, [1 1 2]) .* repmat(s, [1 1 2]);
    return;

end

if strcmp(method, 'propagation')

    % normalize
    d = sqrt(sum(v.^2,3)); d(d<eps) = 1;
    v = v ./ repmat(d,[1 1 2]);
    % padd with zeros
    v0 = zeros(n+2,n+2,2);
    v0(2:end-1,2:end-1,:) = v; v = v0;

    neigh = {[1 0] [-1 0] [0 1] [0 -1]};
    % state
    n1 = n+2;
    S = zeros(n1);
    S(1,:) = -1; S(:,1) = -1; S(end,:) = -1; S(:,end) = -1;
    % initial point
    list = getoptions(options, 'start_points', floor(rand(2,1)*n+1));
    list = list+1;
    Prio = zeros(n1,n1)+Inf;
    for k=1:size(list,2)
        Prio(list(1,k),list(2,k))=0;
    end
    iter = 0;
    while not(isempty(list))
        iter = iter+1;
        progressbar(iter,n^2);
        % clf; imageplot(S); drawnow;
        I = list(1,:) + (list(2,:)-1)*n1;
        prio = Prio(I);
        % extract best match
        [tmp,I] = min(prio);
        i = list(1,I); j = list(2,I);
        prio(I) = []; list(:,I) = [];
        % set to dead
        S(i,j) = -1;
        % compute average vector
        tau = [S(i+1,j); S(i-1,j); S(i,j+1); S(i,j-1)]; tau = tau==-1;
        mtau = sum(tau);
        if mtau==0 && Prio(i,j)~=0 % iter>1
            error('Problem with computations.');
        end
        if mtau>0
            tau = repmat(tau, [1 1 2]);
            av = v(i+1,j,:).*tau(1,1,:) + v(i-1,j,:).*tau(2,1,:) + v(i,j+1,:).*tau(3,1,:) + v(i,j-1,:).*tau(4,1,:);
            av = av / mtau;
            if sum(av.*v(i,j,:))<0
                v(i,j,:) = -v(i,j,:);
            end
        end
        for s=1:length(neigh)
            is = i+neigh{s}(1); js = j+neigh{s}(2);
            w = squeeze(v(i,j,:));
            Prio(is,js) = min( Prio(is,js), Prio(i,j) + 1-abs( sum(w.*neigh{s}')) );
            if S(is,js)==0
                    % add to the front
                    list(:,end+1) = [is;js];
                    S(is,js) = 1; % the front
            end
        end
    end
    progressbar(iter,iter);

    w = v(2:end-1,2:end-1,:);
    w = w .* repmat(d, [1 1 2]);
    return;
end

if strcmp(method, 'localproj')
    % special case.
    for i=1:n
        for j=1:n
            m = zeros(1,1,2);
            if i>1
                m = m + v(i-1,j,:);
            end
            if j>1
                m = m + v(i,j-1,:);
            end
            if i>1 && j>1
                m = m + v(i-1,j-1,:);
            end
            s = dot( m, v(i,j,:) );
            if s<0
                v(i,j,:) = -v(i,j,:);
            end

        end
    end

    w = v;
    return;
end

if strcmp(method, 'randomized')

    % normalize
    d = sqrt(sum(v.^2,3)); d(d<eps) = 1;
    v = v ./ repmat(d,[1 1 2]);

    % padd with zeros
    v0 = zeros(n+2,n+2,2);
    v0(2:end-1,2:end-1,:) = v; v = v0;
    vx = v(:,:,1);
    vy = v(:,:,2);

    % number of iterations
    if isfield(options, 'niter_reorient')
        niter = options.niter_reorient;
    else
        niter = 2000;
    end
    % make 2 sub-grids
    [Y,X] = meshgrid(1:n,1:n);
    I = find( mod(X(:)+Y(:),2)==0 );
    [i1,j1] = ind2sub([n n], I);
    I = find( mod(X(:)+Y(:),2)==1 );
    [i2,j2] = ind2sub([n n], I);

    delta1 = 0.9; delta2 = 0;
    tlist = linspace(0,1,niter);
%    tlist = tlist.^3;

    err = [];
    for k=1:niter
        progressbar(k,niter);
        if mod(k,2)==1
            i = i1+1; j=j1+1;
        else
            i = i2+1; j=j2+1;
        end

        wx = vx( i + (j-1)*(n+2) );
        wy = vy( i + (j-1)*(n+2) );

        zx =    vx( i+1 + (j-1)*(n+2) ) + ...
            vx( i   + (j  )*(n+2) ) + ...
            vx( i-1 + (j-1)*(n+2) ) + ...
            vx( i   + (j-2)*(n+2) );
        zy =    vy( i+1 + (j-1)*(n+2) ) + ...
            vy( i   + (j  )*(n+2) ) + ...
            vy( i-1 + (j-1)*(n+2) ) + ...
            vy( i   + (j-2)*(n+2) );
        % normalize
        dd = sqrt(zx.^2+zy.^2); dd(dd<eps) = 1;
        zx = zx./dd; zy = zy./dd;

        delta = (1-tlist(k))*delta1 + tlist(k)*delta2;

        s = wx.*zx + wy.*zy;
        s = sign(s-delta);
        vx( i + (j-1)*(n+2) ) = wx.*s;
        vy( i + (j-1)*(n+2) ) = wy.*s;
        err(end+1) = sum(s<0);
    end
    v = cat(3,vx,vy);
    w = v(2:end-1,2:end-1,:);
    w = w .* repmat(d, [1 1 2]);
    return;
end

switch lower(method)
    case 'xproj'
        s = v(:,:,1);
    case 'yproj'
        s = v(:,:,2);
    case 'circproj'
        n = size(v,1);
        p = size(v,2);
        [Y,X] = meshgrid(0:p-1, 0:n-1);
        s = v(:,:,1).*X + v(:,:,2).*Y;
    case 'custproj'
        s = v(:,:,1)*w(1) + v(:,:,2)*w(2);
    otherwise
        error('Unknown method');
end

s = sign(s);
I = find(s==0); s(I) = 1;
w = v;
w(:,:,1) = v(:,:,1).*s;
w(:,:,2) = v(:,:,2).*s;


function [eg] = dag_to_essential_graph(dag)
cpdag = dag_to_cpdag(dag);
eg = dag + dag .* (cpdag + cpdag');

return;

% Coverts a DAG into Essential Graph where edges are coded by 2 and 3, 2 is
% directed edge and 3 is bidirected edge and is at one (the same as the original DAG) of the two
% symetrical places.

% Is implemented by the algorithm of Max Chickering in D.M.Chickering (1995).
% A transformational characterization of equivalent Bayesian network structures.
% In Proceedings of Eleventh Conference on Uncertainty in Artificial Intelligence, Montreal, QU,
% pages 87-98. Morgan Kaufmann
% http://research.microsoft.com/~dmax/publications/uai95.pdf

% Implemented by Tomas Kocka, AAU.

function [eg] = dag_to_essential_graph(dagx)

%print_dag(dagx); % Just checking input

order = topological_sort(dagx); % get the topological order of nodes and their number

% fprintf('the topological order is: %d',order);
% fprintf('\n');

[nx,ny] = size(dagx); % gets the number of nodes, note that nx == ny
[I,J] = find(dagx); % finds all nonzero elements in the adjacency matrix, i.e. arcs in the DAG - however we will overwrite it in a special order
% we will sort the arcs from lowest possible y and highest possible x, arcs are x->y
e = 1;
for y = 1:ny
    for x = nx:-1:1
        %fprintf('x %d ',order(x)); fprintf('y %d ',order(y));
        if dagx(order(x),order(y)) == 1
            I(e) = order(x);
            J(e) = order(y);
            e = e + 1;
            %fprintf('x order %d',x);
            %fprintf('y order %d',y);
            %fprintf('\n');
        end
    end
end

% fprintf('the arcs are: %d',I);
% fprintf('\n');
% fprintf('the arcs are: %d',J);
% fprintf('\n');

% Now we have to decide which arcs are part of the essential graph and
% which are undirected edges in the essential graph.
% Undecided arc in the DAG are 1, directed in EG are 2 and undirected in EG
% are 3.

for e = 1:length(I)
    if dagx(I(e),J(e)) == 1
        cont = true;
        for w = 1:nx
            if dagx(w,I(e)) == 2
                if dagx(w,J(e)) ~= 0
                    dagx(w,J(e)) = 2;
                else
                    for ww = 1:nx
                        if dagx(ww,J(e)) ~= 0
                           dagx(ww,J(e)) = 2;
                        end
                    end % and now skip the rest and start with another arc from the list
                    w = nx;
                    cont = false;
                end
            end
        end
        if cont
           exists = false;
           for z = 1:nx
               %fprintf('test %d',dagx(z,J(e)));
               if dagx(z,J(e)) ~= 0 & z ~= I(e) & dagx(z,I(e)) == 0
                  exists = true;
                  for ww = 1:nx
                        if dagx(ww,J(e)) == 1
                           dagx(ww,J(e)) = 2;
                        end
                  end
               end
           end
           if ~ exists
               for ww = 1:nx
                   if dagx(ww,J(e)) == 1
                      dagx(ww,J(e)) = 3;
                   end
               end
           end
        end
    end
end

%print_dag(dagx); % Just checking output


%% OPTI Toolbox Differentiation Demo
%
% This file demonstrates each of the different differentiation algorithms
% supplied with the OPTI Toolbox. Note you should be familiar with the
% operation of OPTI Toolbox by reading the accompanying examples, as well
% as you should have completed the NLP demo.
%
%   Copyright (C) 2011 Jonathan Currie (I2C2)

%% Gradient vs Jacobian
% The terms Gradient and Jacobian are associated with differentiation
% functions, and for the purposes of this toolbox they are basically
% interchangeable:
%
% Gradient - The first derivative of the objective function (1 x n)
% Jacobian - The first derivative of the constraint function (m x n)
%
% Where you will note the main difference is the gradient is a vector, and
% the Jacobian is a matrix. However this is just a rule of thumb and will
% not hold in all instances (constraint functions with one row for
% example!). All functions are named as returning a Jacobian, which could
% also be a Gradient Vector.

%% Problem 1
% For the following examples we are going to use this nonlinear objective
% function:

fun = @(x) sin(pi*x(1)/12) * cos(pi*x(2)/16);

x = [0.75 0.25]';

%% Example 1 - Automatic Differentiation (AD)
% Automatic differentiation is one of the most powerful differentiation
% strategies which can provide error free gradients of a function by
% applying the chain rule to each operation.
%
% The AD routines implemented in OPTI are supplied by adiff, a Matlab
% project by William McIlhagga. While most Matlab functions are overloaded,
% you cannot use external code (i.e. via MEX) or any toolbox or class
% functions.

dx = autoJac(fun,x)

%% Example 2 - Numerical Differentiation (ND)
% Numerical differentiation using finite differences is a computationally
% expensive procedure which can result in an inaccurate gradient if the
% internal perturbations are not chosen correctly.
%
% The ND routine implemented in OPTI is the Intel MKL djacobi function
% which approximates the derivative using central differences. This is
% implemented via a MEX function which repeatedly calls the function in
% order to close in on the gradient.

dx = mklJac(fun,x)

%% Example 3 - Symbolic Differentiation (SD)
% Symbolic differentiation analytically differentiates the function as a
% symbolic expression, resulting in a single expression for the gradient.
% Complications occur if the function cannot be analytically differentiated
% or the symbolic routine cannot find a derivative.
%
% The SD routine implemented in OPTI uses the Matlab Symbolic Toolbox as
% well as two wrapper functions in order to generate the gradient. The
% wrapper functions convert the function handle to a symbolic expression
% and vice-versa.

grad = symJac(fun)

if(~isempty(grad)) %don't run if Symbolic Toolbox not installed
    dx = grad(x)
end

%% Problem 2
% For the next few examples we will be solving this NLP:

obj = @(x) log(1+x(1)^2) - x(2);

lb = [-2 -2]';
ub = [2 2]';

%% Example 4 - Applying AD to NLP Solving
% To use AD to generate the objective gradient for IPOPT create a function
% handle which calls autoJac, and then pass this to the optiprob function:

grad = @(x) autoJac(obj,x);

opts = optiset('solver','ipopt');
Opt = opti('obj',obj,'grad',grad,'bounds',lb,ub,'options',opts)

[x,fval,exitflag,info] = solve(Opt,[0;0]);
fval
info

%% Example 5 - Applying ND to NLP Solving
% To use ND to generate the objective gradient for IPOPT just replace the
% above with mklJac:

grad = @(x) mklJac(obj,x);

Opt = opti('obj',obj,'grad',grad,'bounds',lb,ub,'options',opts)

[x,fval,exitflag,info] = solve(Opt,[0;0]);
fval
info

%% Example 6 - Applying SD to NLP Solving
% To use SD we can generate the analytical gradient and use this as our
% gradient function:

grad = symJac(obj)

if(~isempty(grad))
    Opt = opti('obj',obj,'grad',grad,'bounds',lb,ub,'options',opts)

    [x,fval,exitflag,info] = solve(Opt,[0;0]);
    fval
    info
else
    display('Cannot run example without Symbolic Toolbox!');
end

%% Conclusion
% By default OPTI Toolbox will always use Numerical Differentiation
% (mklJac) for gradients / Jacobians if one has not been provided. This
% provides the best combination of flexibility with complex objective /
% constraint functions and performance. However if you can use SD to
% generate a gradient this would be a preferred method.
%
% If you find the optimizer is failing with ND and AD is a suitable
% candidate you can try it to ensure your gradients are accurate.


function [t,u,v,idx,xnode]=raysurf(p0,v0,node,face)
%
% [t,u,v,idx,xnode]=raysurf(p,v,node,face)
%
% perform a Havel-styled ray tracing for a triangular surface
%
% author: Qianqian Fang, <q.fang at neu.edu>
%
% input:
%   p0: list of starting points of the rays
%   v0: directional vector of the rays,
%   node: a list of node coordinates (nn x 3)
%   face: a surface mesh triangle list (ne x 3)
%
% output:
%   t: distance from p0 to the intersection point for each surface
%      triangle, if t(i)=NaN, no intersection was found for that ray
%   u: bary-centric coordinate 1 of all intersection points
%   v: bary-centric coordinate 2 of all intersection points
%      the final bary-centric triplet is [u,v,1-u-v]
%   idx: idx lists the IDs of the face elements that intersects
%      each ray
%   xnode: optional output, if requested, xnode gives the intersection
%      point coordinates; to compute manually, xnode=p0+repmat(t,1,3).*v0
%
% Reference:
%  [1] J. Havel and A. Herout, "Yet faster ray-triangle intersection (using
%          SSE4)," IEEE Trans. on Visualization and Computer Graphics,
%          16(3):434-438 (2010)
%  [2] Q. Fang, "Comment on 'A study on tetrahedron-based inhomogeneous
%          Monte-Carlo optical simulation'," Biomed. Opt. Express, (in
%          press)
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

len=size(p0,1);
if(len==0)
   error('p0 can not be empty');
end
if(size(node,2)<3)
   error('node must contain at least 3 columns');
end
if(size(face,2)<3)
   error('face must contain at least 3 columns');
end

if(size(v0,1)==1 || size(v0,2)==1 && len>1)
   v0=repmat(v0(:)',len,1);
end

t=zeros(len,1)*nan;
u=t;
v=t;
idx=t;

for i=1:len
   [ti,ui,vi,id]=raytrace(p0(i,:),v0(i,:),node,face);
   if(isempty(id)) continue; end
   ti=ti(id);
   tpid=find(ti>=0);
   if(isempty(tpid)) continue; end
   [tmin,tloc]=min(ti(find(ti>=0)));
   t(i)=tmin;
   u(i)=ui(id(tpid(tloc)));
   v(i)=vi(id(tpid(tloc)));
   idx(i)=id(tpid(tloc));
end

if(nargout>=5)
   xnode=p0+repmat(t,1,3).*v0;
end


function [X_out] = etrf2000itrf2008(X_in, date)

% SYNTAX:
%   [X_out] = etrf2000itrf2008(X_in, date);
%
% INPUT:
%   X_in = input coordinates (XYZ)
%   date = reference date
%
% OUTPUT:
%   X_out = output coordinates (XYZ)
%
% DESCRIPTION:
%   ETRF2000 to ITRF2008 coordinate converter.

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:
%  Contributors:     ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

%decimal year (YYYY.DDD)
[~, frac] = date2doy(datenum(date));
t = date(1) + frac;

%ITRF2008 - ETRF2000 parameters
% (http://etrs89.ensg.ign.fr/memo-V8.pdf)

%translation [mm]
T1 =  52.1; T1dot =  0.1;
T2 =  49.3; T2dot =  0.1;
T3 = -58.5; T3dot = -1.8;

%scale factor
D = 1.34e-9; Ddot = 0.08e-9;

%rotation [mas]
R1 =  0.891; R1dot =  0.081;
R2 =  5.390; R2dot =  0.490;
R3 = -8.712; R3dot = -0.792;

%propagate parameters to epoch t
T1 = T1 + T1dot*(t - 2000.0);
T2 = T2 + T2dot*(t - 2000.0);
T3 = T3 + T3dot*(t - 2000.0);
D  = D  + Ddot *(t - 2000.0);
R1 = R1 + R1dot*(t - 2000.0);
R2 = R2 + R2dot*(t - 2000.0);
R3 = R3 + R3dot*(t - 2000.0);

%convert translation parameters to [m]
T1 = T1 * 1e-3;
T2 = T2 * 1e-3;
T3 = T3 * 1e-3;

%convert rotation parameters to [rad]
R1 = R1 * 4.848136e-9;
R2 = R2 * 4.848136e-9;
R3 = R3 * 4.848136e-9;

%translation vector
T = [T1; T2; T3];

%rotation/scale matrix
R = [1 -R3 R2; R3 1 -R1; -R2 R1 1];

%7-parameters Helmert inverse transformation
X_out = inv(R)/(1+D)*(X_in - T);


function s = y2s(y)
% S = y2s(Y)
%
% Admittance to Scattering transformation
% for square matrices at multiple frequencies
%
% s = inv(I+y) * (I-y)
% ver 0.0 original	31.03.1998
% ver 0.1 +freq		27.09.2002
% ver 0.2 +octave supp.	01.06.2005

  if size(size(y),2) > 2
      nF = size(y,3);
    else nF = 1;
    end;

I = diag(ones(1, size(y,2)));

  for i=1:nF
    s(:,:,i) = inv(I+y(:,:,i)) * (I-y(:,:,i));
  end;


function [Ft3]=tri6_subtri3(Ft6,Vt6)

% function [Ft3]=tri6_subtri3(Ft6,Vt6)
% ------------------------------------------------------------------------
% The input 6-node triangles are either oriented such that the first node
% is a corner node or that the first node is a mid-edge node. The two cases
% are highlighted below.
%
% Case I
%                     1
%                    / \
%                   2   6
%                  /     \
%                 3___4___5
% Case II
%                     6
%                    / \
%                   5   1
%                  /     \
%                 4___3___2
%
% The output will follow one of these rules and maintains face normals
% directions:
%
% Case I
%                     1
%                    / \
%                   2___6
%                  / \ / \
%                 3___4___5
%
% Case II
%                     6
%                    / \
%                   5___1
%                  / \ / \
%                 4___3___2
%
% ------------------------------------------------------------------------
%%
%Check face order based on first
a=Vt6(Ft6(1,2),:)-Vt6(Ft6(1,1),:); %2 1 edge
b=Vt6(Ft6(1,6),:)-Vt6(Ft6(1,1),:); %6 1 edge
c=Vt6(Ft6(1,3),:)-Vt6(Ft6(1,2),:); %3 2 edge

am=sqrt(sum(a.^2));
bm=sqrt(sum(b.^2));
cm=sqrt(sum(c.^2));

phi=abs(acos(dot(a,b)./(am*bm)));
phi=mod(phi,pi); %Angle between 2 1 and 6 1 edge
theta=abs(acos(dot(a,c)./(am*cm)));
theta=mod(theta,pi); %Angle between 2 1 and 3 2 edge

if phi>theta
    Ft3=[Ft6(:,[1 2 6]); Ft6(:,[2 3 4]); Ft6(:,[4 5 6]); Ft6(:,[2 4 6])]; %Clockwise
else
    Ft3=[Ft6(:,[1 2 3]); Ft6(:,[3 4 5]); Ft6(:,[5 6 1]); Ft6(:,[1 3 5])]; %Anti-clockwise
end

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


function DEM_FEP_Least_Action
%--------------------------------------------------------------------------
% This routine uses a Lorenz system to show that the most likely autonomous
% path (or deterministic path) is uniquely identified by its initial and
% final states. In other words, if we knew the end state of an autonomous
% trajectory, then we would implicitly know the path taken from the initial
% particular state, even if we did not know the external states. This point
% is demonstrated using numerical analyses of the Lorenz system; treating
% the first state and an active state and the third as an external state.
% In this example, 1024 solutions are obtained from the same initial
% particular (i.e., sensory and active) states but sampling from a Gaussian
% distribution over external states. The ensuing trajectories over 128 time
% bins of 1/128 seconds are shown in the left panels. The sample
% distribution over active states is shown as a (scaled) histogram along
% the x-axis. Paths that end within 1/8 of an arbitrary active state (here,
% ?? = -4) are shown in red. The corresponding autonomous (i.e., active)
% paths are shown as a function of time in the right panels. one can repeat
% this analysis for different levels of random fluctuations; e.g.,log
% precisions of 2 and 16. The key thing to observe is that as the amplitude
% of random fluctuations decreases (i.e., its precision increases) the
% paths that begin and end in the same place collapse to a single
% trajectory of least action. This is the most likely or deterministic
% path. Clearly, this behaviour rests upon a diffeomorphic mapping between
% the initial and final states: for example, a final active state of -8 has
% the least two paths of least action (xT in the code below).
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: DEM_FEP_Least_Action.m 7512 2019-01-05 21:15:16Z karl $

% generative model
%==========================================================================                       % switch for demo
spm_figure('GetWin','DEM'); clf

% flow
%--------------------------------------------------------------------------
dt    = 1/128;
G.f   = @(x,v,P,G) v(:) + [-P(1) P(1) 0; P(3) -1 -x(1); x(2) 0 P(2)]*x*dt;

% set up
%--------------------------------------------------------------------------
T     = 128;                        % length of trajectory
N     = 1024;                       % number of paths
g     = exp(-8);                    % amplitude of random fluctuations
s     = exp(2);                     % amplitude deviations
P     = [10; -8/3; 28];             % Rayleigh parameter
x0    = [1; 1; 25];
for k = 1:N

    % random fluctuations (and action)
    %----------------------------------------------------------------------
    U.u      = randn(T,3)*g;
    A(k)     = (1/2)*sum(U.u(:).^2)/(g^2)/T;

    % random deviations in external states (and probability)
    %----------------------------------------------------------------------
    dx       = randn*s;
    A(k)     = A(k) + (1/2)*sum(dx.^2)/(s^2);

    % integrate timeseries with random initial hidden state
    %----------------------------------------------------------------------
    G.x      = x0;
    G.x(3)   = x0(3) + dx;
    x(:,:,k) = spm_int_L(P,G,U);

end

% plot ensemble densities
%--------------------------------------------------------------------------
subplot(2,2,1),cla, hold on
td    = fix(linspace(1,N,32));
for t = td
    plot(x(:,1,t),x(:,3,t),':','MarkerSize',1)
    plot(x(T,1,t),x(T,3,t),'r.','MarkerSize',8)
end
axis square, axis([-20 20 0 60])
title('Paths from initial state','FontSize',16)
xlabel('active state'),ylabel('external state'),drawnow

% find trajectories that start and end at the same place
%==========================================================================

% get surprisal of final (active) state using sample density
%--------------------------------------------------------------------------
nb    = 32;
xN    = squeeze(x(end,1,:));
[n,a] = hist(xN,nb);
n     = 2*nb*n/sum(n);
bar(a,n,1)

% identify and plot trajectories with the same endpoints
%--------------------------------------------------------------------------
xT    = -4;
k     = find(abs(xN - xT) < 1/8);
for t = k(:)'
    plot(x(:,1,t),x(:,3,t),'r')
end
plot([x0(1),x0(1)],[0 48],'r-.')
plot([xT   ,xT   ],[0 48],'r-.')

% paths as a function of time
%--------------------------------------------------------------------------
subplot(2,2,2), hold on
pst   = (1:T)*dt;
for t = k(:)'
    plot(pst,x(:,1,t),'r')
end
title('Autonomous paths','FontSize',16)
xlabel('time (seconds)'),ylabel('active state'),axis square

return

% NB: numerical analysis of the action and surprisal
%--------------------------------------------------------------------------
for i = 1:N
    [d,j] = min(abs(xn - xN(i)));
    L(i)  = -log(n(j));
end
L      = log(spm_softmax(L(:)));
A      = log(spm_softmax(A(:)));

subplot(2,2,2)
plot(L,A,'.',L,L,':')
title('Action and surprisal','FontSize',16)
xlabel('Surprisal of final state'),ylabel('Action of path'),axis square


% Author: Ricardo Baptista and Matthias Poloczek
% Date:   June 2018
%
% See LICENSE.md for copyright information
%

function weights_new = importance_weights(particle_system, new_rho)
% IMPORTANCE_WEIGHTS: Function updates the importance weights for each
% binary model based on the geometric bridge model and the empirical
% distribution

% Extract inputs to function
models     = particle_system.models;
model_val  = particle_system.model_val;
old_wts    = particle_system.weights;
old_rho    = particle_system.rho;

% Determine the number of models and trials
[n_models, ~] = size(models);

% Declare a vector to store weights
weights_new = zeros(n_models,1);

for i=1:n_models

    % Evaluate posterior ratios: pi(M) \propto exp(rho*f(M))
    post_new_rho = new_rho*model_val(i);
    post_old_rho = old_rho*model_val(i);

    % Find the weights corresponding to each model
    weights_new(i) = old_wts(i)*exp(post_new_rho - post_old_rho);

end

% Check for NaNs and set weight to zero
nan_weight = isnan(weights_new);
weights_new(nan_weight) = 0;

% Renormalize weights
weights_new = weights_new/sum(weights_new);

end


function v = lexicmp(a,b, tol)

% compare using lexicographical ordering the arrays
%
%   v = lexicmp(a,b, tol);
%
%   tol (default 1e-9) is a tolerance of equality.
%
%   return -1 if a<b, +1 if a>b, 0 if a=b.
%
%   Copyright (c) 2008 Gabriel Peyre

tol = 1e-9;
a = a(:);
b = b(:);

n = max(length(a),length(b));
a(n+1:end) = -Inf;
b(n+1:end) = -Inf;

for i=1:n
    if a(i)<b(i)-tol
        v = -1; return;
    end
    if a(i)>b(i)+tol
        v = +1; return;
    end
end
v = 0;


function [ nodes_per_layer, n ] = hex_grid_approximate_n ( box, n_goal )

%*****************************************************************************80
%
%% HEX_GRID_APPROXIMATE_N seeks a hex grid of about N nodes.
%
%  Discussion:
%
%    The parameter NODES_PER_LAYER controls the number of nodes, but
%    in a somewhat obscure way.  This routine experiments with various
%    values until it is convinced it has the value of NODES_PER_LAYER
%    that comes as close as possible to producing N nodes.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 March 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real BOX(2,2), the lower and upper corners
%    of the rectangular region.
%
%    Input, integer N_GOAL, the desired number of nodes.
%
%    Output, integer NODES_PER_LAYER, the number of nodes per layer
%    which produces a mesh with about N_GOAL nodes.
%
%    Output, integer N, the number of nodes in the mesh.
%
  m = 2;

  if ( n_goal <= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'HEX_GRID_APPROXIMATE_N - Fatal error!\n' );
    fprintf ( 1, '  Illegal input value of N_GOAL = %d\n', n_goal );
    error ( 'HEX_GRID_APPROXIMATE_N - Fatal error!' );
  end

  nodes_per_layer_low = 0;
  n_low = 0;

  nodes_per_layer = round ( sqrt ( n_goal ) );

  nodes_per_layer_high = n_goal;
  n_high = n_goal * n_goal;

  while ( 1 )

    n = hex_grid_n ( nodes_per_layer, box );

    if ( n == n_goal )
      break
    end

    if ( n < n_goal )
      nodes_per_layer_low = nodes_per_layer;
      n_low = n;
    else
      nodes_per_layer_high = nodes_per_layer;
      n_high = n;
    end

    if ( nodes_per_layer_low + 1 == nodes_per_layer_high )
      if ( n - n_low <= n_high - n )
        nodes_per_layer = nodes_per_layer_high;
        n = n_high;
      else
        nodes_per_layer = nodes_per_layer_low;
        n = n_low;
      end
      break
    end

    nodes_per_layer = ...
      round ( ( nodes_per_layer_low + nodes_per_layer_high ) / 2 );

  end

  return
end


function [ly] = nm2ly(nm)
% Convert length from nanometers to light years.
% Chad A. Greene 2012
ly = nm*1.057000834025e-25;


function [M] = ft_connectivity_mim(inputdata, varargin)

% FT_CONNECTIVITY_MIM computes the multivariate interaction measure from a
% data-matrix containing the cross-spectral density. This implements the method
% described in Ewald et al., Estimating true brain connectivity from EEG/MEG data
% invariant to linear and static trasformations in sensor space. Neuroimage, 2012;
% 476:488.
%
% Use as
%   [m] = hcp_connectivity_mim(inputdata, ...)
%
% The input data should be an array organized as
%   Channel x Channel x Frequency
%
% The output m contains the newChannel x newChannel x Frequency connectivity measure,
% with newChannel equal to max(indices).
%
% Additional optional input arguments come as key-value pairs:
%   'indices' = 1xN vector with indices of the groups to which the channels belong,
%               e.g. [1 1 2 2] for a 2-by-2 connectivity between 2 planar MEG channels.
%
%
% See also CONNECTIVITY, FT_CONNECTIVITYANALYSIS

% Copyright (C) 2011-2014 by the Human Connectome Project, WU-Minn Consortium (1U54MH091657)
% Copyright (C) 2021 Jan-Mathijs Schoffelen, DCCN
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

indices = ft_getopt(varargin, 'indices');

if isempty(indices) && isequal(size(inputdata(:,:,1)), [2 2])
  % simply assume two channels
  indices = [1 1 2 2];
end

sizein  = size(inputdata);
sizeout = [sizein 1];
sizeout(1:2) = max(indices);

% compute the inverse of the auto terms only once for speed up
invC = cell(sizeout(1),sizeout(3));
for kk = 1:sizeout(3)
  for k = 1:sizeout(1)
    invC{k,kk} = pinv(real(inputdata(indices==k,indices==k,kk)));
  end
end

M = zeros(sizeout);
for kk = 1:sizeout(3)
  for k = 1:sizeout(1)
    for m = 1:sizeout(1)
      indx1 = indices==k;
      indx2 = indices==m;
      %cs_aa_re = real(input(indx1,indx1));
      %cs_bb_re = real(input(indx2,indx2));
      cs_ab_im = imag(inputdata(indx1,indx2,kk));

      %inv_cs_bb_re = pinv(cs_bb_re);
      %inv_cs_aa_re = pinv(cs_aa_re);
      inv_cs_bb_re = invC{m,kk};
      inv_cs_aa_re = invC{k,kk};
      transp_cs_ab_im = transpose(cs_ab_im);
      M(k,m,kk) = trace(inv_cs_aa_re*cs_ab_im*inv_cs_bb_re*transp_cs_ab_im); % try to speed up by dividing calculation in steps
    end
  end
end

% taking the mldivide and mrdivide operators doesn't change the results, but speeds up by a factor of 4 over 1000 iterations (on LM Notebook)
% m = trace(cs_aa_re\cs_ab_im*inv_cs_bb_re*transp_cs_ab_im);

% % % block_a=cs_aa_re\cs_ab_im;
% % % block_b=cs_bb_re\transpose(cs_ab_im);
% % % m = trace(block_a*block_b);

% m = trace(cs_aa_re\cs_ab_im*(cs_bb_re\transpose(cs_ab_im)));


function out = PeubSource(k, p, d, L, accuracy)
%upper bound for the error probability of the binary source
%achievability via sphere covering
%k - source blocklength
%p - source bias
%d - distortion threshold
%L = log M, where M is the number of representation points
%accuracy = accuracy of Csum: 1 - more accurate, 0 - faster

%
%   Created in 2012 by Victoria Kostina (vkostina@caltech.edu)
%

Kbig = 80;

if accuracy < 0
    %normal approximation
    Rd = h(p) - h(d);
    Vs = p*(1-p)*(log2((1-p)/p))^2;
    if p ~= 1/2 && k >= Kbig
        out = Q( (L - k*Rd - log2(k))/sqrt(k*Vs) );
    else
        out = PeubSource(L, k, p, d);
    end
    return;
end

if d == 0
    %lossless
    out = PeubSourceLossless(L, k, p);
elseif p == 1/2
    %FAIR coin flip source
    out = PeubSourceFair(L, k, d);
else
    %arbitrary p source
    out = PeubSourceBiased(L, k, p, d);
end
%        fprintf('SphereCoveringA: L = %i, k = %i, Peub = %f\n', L, k, out);

    function out = PeubSourceLossless(L, k, p)
        csum = 0;
        kstar = k;
        for i = 0:k
            csum = csum + C(k, i, 'ub');
            if log2(csum) > L
                kstar = i - 1;
                break;
            end
        end
        out =  1 - binocdf(kstar,k,p);
    end

    function out = PeubSourceFair(L, k, d)
        if k <= Kbig
            out =  2^L *log(1 - Csum(k, floor(k*d), 'lb', accuracy) /(2^k));
        else
            out = -2^(L-k)*Csum(k, floor(k*d), 'lb', accuracy);
        end
        out = exp(out);
    end

    function out = PeubSourceBiased(L, k, p, d)
        q = max ( 0, (p-d)/(1-2*d));
        if q == 0
            q = p;
        end
        out = 0;
        for t = 0:k
            out = out + binopdfbound(t,k,p, 'ub')*e(k,t);
            if isnan(out)
                break;
            end
        end

        function out = e(k,t)
            out = 0;
            if 1
                for T = 0:k
                    out = out + binopdfbound(T, k, q, 'ub')*Pwithin(k, t, T);
                end
            else
                T = round(q*k); %#ones
                out = Pwithin(k, t, T);
            end

            if k <= Kbig
                out = 2^L*log( 1 - out);
                out = exp(out);
            else
                out = exp(-2^L*out);
            end

        end

        function out = Pwithin(k,t, T)
            %y of type T is within a given x of type t - almost exact lower bound
            t0 = max ( 0, ceil( (t+T - k*d)/2) ); %# ones  in 11111 zone (T ones in codewords)

            out = logC(T, t0, 'lb') + logC(k-T, t - t0, 'lb') - logC(k, t, 'ub');
            out = 2^out;
        end

    end
end


function y = BF_PreProcess(y,preProcessHow)
% BF_PreProcess   Preprocess a time series, y

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

if ~isempty(preProcessHow)
    switch preProcessHow
    case 'diff1'
        % Takes incremental differences of the input time series
        y = diff(y);
    case 'rescale_tau'
        % Coarse-graining at a given scale, as in multiscale entropy approaches
        % Find first zero of the autocorrelation function
        tau = CO_FirstCrossing(y,'ac',0,'discrete');
        % Buffer the time series into nonoverlapping windows of length tau
        y_buffer = BF_MakeBuffer(y,tau);
        % Mean each window to get a coarse-grained time series
        y = mean(y_buffer,2);
    otherwise
        error('Unknown preprocessing setting: ''%s''',preProcessHow);
    end
end

end


function x = blend_rst_0dn ( r, s, t, n, bound_rst )

%*****************************************************************************80
%
%% BLEND_RST_0DN extends vector data at corners into a cube.
%
%  Diagram:
%
%    010-----r10-----110        011-----r11-----111
%      |       .       |          |       .       |
%      |       .       |          |       .       |
%    0s0.....rs0.....1s0        0s1.....rs1.....1s1     S
%      |       .       |          |       .       |     |
%      |       .       |          |       .       |     |
%    000-----r00-----100        001-----r01-----101     +----R
%           BOTTOM                      TOP
%
%    011-----0s1-----001        111-----1s1-----101
%      |       .       |          |       .       |
%      |       .       |          |       .       |
%    01t.....0st.....00t        11t.....1st.....10t          T
%      |       .       |          |       .       |          |
%      |       .       |          |       .       |          |
%    010-----0s0-----000        110-----1s0-----100     S----+
%           LEFT                       RIGHT
%
%    001-----r01-----101        011-----r11-----111
%      |       .       |          |       .       |
%      |       .       |          |       .       |
%    00t.....r0t.....100        01t.....r1t.....11t     T
%      |       .       |          |       .       |     |
%      |       .       |          |       .       |     |
%    000-----r00-----100        010-----r10-----110     +----R
%           FRONT                       BACK
%
%  Discussion:
%
%    BLEND_RST_0DN is equivalent to a trilinear finite element method.
%    Data along the edges, faces, and interior of the cube is
%    interpolated from the data at the corners.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    William Gordon,
%    Blending-Function Methods of Bivariate and Multivariate Interpolation
%    and Approximation,
%    SIAM Journal on Numerical Analysis,
%    Volume 8, Number 1, March 1971, pages 158-177.
%
%    William Gordon and Charles Hall,
%    Transfinite Element Methods: Blending-Function Interpolation over
%    Arbitrary Curved Element Domains,
%    Numerische Mathematik,
%    Volume 21, Number 1, 1973, pages 109-129.
%
%    William Gordon and Charles Hall,
%    Construction of Curvilinear Coordinate Systems and Application to
%    Mesh Generation,
%    International Journal of Numerical Methods in Engineering,
%    Volume 7, 1973, pages 461-477.
%
%    Joe Thompson, Bharat Soni, Nigel Weatherill,
%    Handbook of Grid Generation,
%    CRC Press, 1999.
%
%  Parameters:
%
%    Input, real R, S, T, the (R,S,T) coordinates of the
%    point to be evaluated.
%
%    Input, integer N, the dimension of the vector space.
%
%    External, BOUND_RST, is a function which is given (R,S,T)
%    coordinates and an component value I, and returns XI, the value
%    of the I-th component of the N-vector at that point.  BOUND_RST
%    will only be called for "corners", that is, for values (R,S,T)
%    where R, S and T are either 0.0 or 1.0.  BOUND_RST has the form:
%      function xi = bound_rst ( r, s, t, i )
%
%    Output, real X(N), the interpolated value at the
%    point (R,S,T).
%
  for i = 1 : n
%
%  Get the I-th coordinate component at the corners.
%
    x000 = bound_rst ( 0.0, 0.0, 0.0, i );
    x001 = bound_rst ( 0.0, 0.0, 1.0, i );
    x010 = bound_rst ( 0.0, 1.0, 0.0, i );
    x011 = bound_rst ( 0.0, 1.0, 1.0, i );
    x100 = bound_rst ( 1.0, 0.0, 0.0, i );
    x101 = bound_rst ( 1.0, 0.0, 1.0, i );
    x110 = bound_rst ( 1.0, 1.0, 0.0, i );
    x111 = bound_rst ( 1.0, 1.0, 1.0, i );
%
%  Interpolate the I-th coordinate component at the edges.
%
    xr00 = blend_101 ( r, x000, x100 );
    xr01 = blend_101 ( r, x001, x101 );
    xr10 = blend_101 ( r, x010, x110 );
    xr11 = blend_101 ( r, x011, x111 );

    x0s0 = blend_101 ( s, x000, x010 );
    x0s1 = blend_101 ( s, x001, x011 );
    x1s0 = blend_101 ( s, x100, x110 );
    x1s1 = blend_101 ( s, x101, x111 );

    x00t = blend_101 ( t, x000, x001 );
    x01t = blend_101 ( t, x010, x011 );
    x10t = blend_101 ( t, x100, x101 );
    x11t = blend_101 ( t, x110, x111 );
%
%  Interpolate the I-th component on the faces.
%
    x0st = blend_112 ( s, t, x000, x001, x010, x011, x0s0, x0s1, x00t, x01t );

    x1st = blend_112 ( s, t, x100, x101, x110, x111, x1s0, x1s1, x10t, x11t );

    xr0t = blend_112 ( r, t, x000, x001, x100, x101, xr00, xr01, x00t, x10t );

    xr1t = blend_112 ( r, t, x010, x011, x110, x111, xr10, xr11, x01t, x11t );

    xrs0 = blend_112 ( r, s, x000, x010, x100, x110, xr00, xr10, x0s0, x1s0 );

    xrs1 = blend_112 ( r, s, x001, x011, x101, x111, xr01, xr11, x0s1, x1s1 );
%
%  Interpolate the I-th coordinate component of the interior point.
%
    x(i) = blend_123 ( r, s, t, x000, x001, x010, x011, x100, x101, x110, x111, ...
      xr00, xr01, xr10, xr11, x0s0, x0s1, x1s0, x1s1, x00t, x01t, x10t, x11t, ...
      x0st, x1st, xr0t, xr1t, xrs0, xrs1 );

  end

  return
end


function model = gmm_estimate(data)
% gmm_estimate Estimates a GMM on a set of points
%
% Originally a part of: Maggot (developed within EU project CogX)
% Original author: Matej Kristan, 2009
%
% Input:
% - data (matrix): Points for which to estimate a model
%
% Output:
% - model (vector): values for corresponding points
%

N = length(data);
model.Mu = data;
model.Cov{N} = cell(N, 1);
for i = 1:N
    model.Cov{i} = 0;
end
model.w = ones(1,N) / N;

pdf0 = model;

% first we'll spherize the distribution
[Mu, C] = spherize(pdf0.Mu, pdf0.Cov, pdf0.w) ;
[U, S, V] = svd(C) ;
T = (diag(1./sqrt(diag(S))))*U' ;

pdf0.Mu = bsxfun(@minus, pdf0.Mu, Mu) ;
pdf0.Mu = T * pdf0.Mu ;
for i = 1 : length(pdf0.w)
    pdf0.Cov{i} = T*pdf0.Cov{i}*T' ;
end

C = T*C*T' ;
% calculate the optimal bandwidth by Kristan's estimator
H = optimal_bandwidth( pdf0.Mu, pdf0.Cov, pdf0.w, C, length(pdf0.w)) ;

pdf.Mu = Mu ;
pdf.Cov = {H} ;
pdf.w = 1 ;

iT = inv(T) ;
pdf.Mu = iT * pdf.Mu ;
pdf.Mu = bsxfun(@minus, pdf.Mu, -Mu) ;
for i = 1 : length(pdf.w)
    pdf.Cov{i} = iT * pdf.Cov{i} * iT';
end
H = pdf.Cov{1} ;

for i = 1:N
    model.Cov{i} = H ;
end

end

function [new_mu, new_Cov] = spherize(Mu, Cov, w)

if length(w)==1
    new_mu = Mu ;
    if ~isempty(Cov)
        new_Cov = Cov{1} ;
    else
        new_Cov = zeros(size(new_mu,1),size(new_mu,1)) ;
    end
    return ;
end

sumw = sum(w) ;
w = w / sumw ;
new_mu =  sum(bsxfun(@times,Mu, w),2) ;

n = size(new_mu,1) ;

if ~isempty(Cov)
    new_Cov = zeros(n,n) ;
    if n==1
        new_Cov = sum(w.*(cell2mat(Cov) + Mu.*Mu)) ;
    else
        for j=1:length(w)
            new_Cov = new_Cov + w(j)*( Cov{j} + Mu(:,j)*Mu(:,j)') ;
        end
    end
    new_Cov = new_Cov - new_mu*new_mu' ;
else
    new_Cov = zeros(n,n) ;
    if n==1
        new_Cov = sum(w.*(Mu.*Mu)) ;
    else
        for j=1:length(w)
            new_Cov = new_Cov + w(j)*(   Mu(:,j)*Mu(:,j)') ;
        end
    end
    new_Cov = new_Cov - new_mu*new_mu' ;

end

end

function [H] = optimal_bandwidth( Mu, Cov, w, Cov_smp, N_eff )

d = size(Mu,1) ;
G = (Cov_smp *(4 / ((d + 2) * N_eff))^(2 / (d + 4)));

alpha_scale = 1 ;
F = Cov_smp * alpha_scale; % for numerical stability. it could have been: F = Cov_smp;
% could also constrain to say that F = identity!
Rf2 = integral_squared_hessian(Mu, w, Cov, F, G);

h_amise = (N_eff ^ (-1) * det(F)^(-1 / 2) /( sqrt(4 * pi) ^ d * Rf2 * d ))^(1 / (d + 4)) ;
H = (F * h_amise ^ 2) * alpha_scale ;

end

function I = integral_squared_hessian(Mu, w, Cov, F, G)
% Calculates an integral over the squared Hessian of a Gaussian mixture
% model.
% Follows Wand and Jones "Kernel Smoothing", page 101., assuming H=h*F.

I = NaN ;
if ( isempty(Mu) )
    return;
end
% read dimension and number of components
[ d, N ]= size(Mu) ;

% precompute normalizer constNorm = ( 1 / 2pi)^(d/2)
constNorm = (1/(2*pi))^(d/2) ;
I = 0 ;

% test if F is identity for speedup
delta_F = sum(sum(abs(F-eye(size(F))))) ;
if delta_F < 1e-3
    % generate a summation over the nonsymmetric matrix
    for l1 = 1 : N
        S1 = Cov{l1}  + G ;
        Mu1 = Mu(:,l1) ;
        w1 = w(l1) ;
        for l2 = l1 : N
            S2 = Cov{l2};
            Mu2 = Mu(:,l2) ;
            w2 = w(l2) ;
            A = inv(S1 + S2) ;
            dm = (Mu1 - Mu2) ;
            m = dm'*A*dm ;
            f_t = constNorm*sqrt(det(A))*exp(-0.5*m) ;
            c = 2*sum(sum(A.*A'))*(1-2*m) + (1-m)^2 *trace(A)^2 ;

            % determine the weight of the term current
            if ( l1 == l2 )
                eta = 1 ;
            else
                eta = 2 ;
            end
            I = I + f_t*c*w2*w1*eta ;
        end
    end
else
    % generate a summation over the nonsymmetric matrix
    for l1 = 1 : N
        S1 = Cov{l1} ;
        Mu1 = Mu(:,l1) ;
        w1 = w(l1) ;
        for l2 = l1 : N
            S2 = Cov{l2} + G;
            Mu2 = Mu(:,l2) ;
            w2 = w(l2) ;
            A = inv(S1 + S2) ;
            dm = (Mu1 - Mu2) ;
            ds = dm'*A ;
            b = ds'*ds ;
            B = A - 2*b ;
            C = A - b ;

            f_t = constNorm*sqrt(det(A))*exp(-0.5*ds*dm) ;
            c = 2*trace(F*A*F*B) + trace(F*C)^2 ;

            % determine the weight of the term current
            if ( l1 == l2 )
                eta = 1 ;
            else
                eta = 2 ;
            end
            I = I + f_t*c*w2*w1*eta ;
        end
    end
end

end


function [fx] = f_metaToM(x,P,u,inF)
% evolution function for meta-learning (k-ToM vs sequence)
% function [fx] = f_metaToM(x,P,u,inF)
% This function update the belief of an agent who has to identify whether
% he faces an intentional agent (k-ToM) or an inanimate patter (sequence).
% Learning here derives from VB-Laplace update rules of the agent's
% posterior belief. practically speaking, this function (i) wraps modified
% evolution functions of both k-ToM learning and sequence learning, and
% (ii) updates the probability Pi that the agent is facing an intentional
% agent given a new observation u (the probability that the agent faces an
% inanimate pattern is 1-Pi).
% IN:
%   - x: hidden states (see indexing in inF.indlev)
%   - P: evolution params:
%   P(1)= agent's prior opponent's (log-) volatility on opponent's params
%   P(2)= agent's (invsigmoid-) dilution coefficient
%   - u: sequence of past actions:
%       u(1)= opponent's last move
%       u(2)= learner's last move
%       u(3:K+2) = sequence of K past opponent's moves
%   - inF: input structure (see prepare_metaToM.m)
% OUT:
%   - fx: updated hidden states
% [see RecToMfunction.m and f_BSL.m]

ot = u(1); % opponent's last move
fx = NaN(size(x)); % initialize updated states

% 1- update P(agent=kToM)
Pi0 = VBA_sigmoid(x(inF.meta.indx)); % prior P(agent=1)
% partial forgetting of prior belief on opponent's type?
if inF.meta.diluteP
    dc = VBA_sigmoid(P(inF.meta.indP)); % dilution coefficient
    Pi0 = (1-dc).*Pi0 + dc./2;
end

% Get k-ToM's likelihood, ie P(o|k-ToM).
xktom = x(inF.ktom.indx);
inFktom = inF.ktom.inF;
ntotPar = inFktom.indParev+inFktom.indParobs; % total number of params
level = inFktom.lev; % depth of k-ToM's recursive beliefs (k=level)
indlev = defIndlev(level,ntotPar); % states indexing
if level==0 % 0-ToM [should be useless]
    mx = xktom(1); % E[log-odds of P(o=1)]
    Vx = exp(xktom(2)); % V[log-odds of P(o=1)]
    Els1 = VBA_Elogsig(mx,Vx);
    Els0 = VBA_Elogsig(-mx,Vx);
    ELL = ot.*Els1 + (1-ot).*Els0;
    h_kToM = exp(ELL); % P(o|k-ToM)
else
    Pk = VBA_sigmoid(x(1:(level-1))); % P(k'), with k'=0,...,k-1
    Pk = [Pk;max(0,1-sum(Pk))]; % insert last P(k'=k-1)
    f = zeros(level,1); % E[x(theta)]
    Vx = zeros(level,1); % V[x(theta)]
    for j=1:level % loop over possible opponent's levels  (k'=j-1)
        f(j) = xktom(indlev(j).f); % E[x(theta)|k'=j-1]
        df = xktom(indlev(j).df); % d[x(theta)]/dtheta for k'=j-1
        Sig = exp(xktom(indlev(j).Par(2:2:2*ntotPar))); % V[theta|k'=j-1]
        Vx(j) = sum(Sig.*df.^2); % V[x(theta)|k'=j-1]
    end
    Els1 = VBA_Elogsig(f,Vx);
    Els0 = VBA_Elogsig(-f,Vx);
    ELL = ot.*Els1 + (1-ot).*Els0;
    h_kToM = exp(Pk'*ELL); % P(o|k-ToM)
end

% Get sequence's likelihood, ie P(o|seq).
xseq = x(inF.seq.indx);
inFseq = inF.seq.inF;
useq = u; % remove agent's previous move
useq(2) = [];
K = inFseq.K; % sequence depth
yb = useq(2:K+1); % previous outcomes
if VBA_isWeird (yb)
    h_seq = 1/2;
else
    if K >0
        indSeq = bin2dec(num2str(yb'))+1; % index of sequence of previous outcomes
    else
        indSeq = 1;
    end
    m = xseq(indSeq);
    v = exp(xseq((2^K)+indSeq));
    Els1 = VBA_Elogsig(m,v);
    Els0 = VBA_Elogsig(-m,v);
    ELL = ot.*Els1 + (1-ot).*Els0;
    h_seq = exp(ELL); % P(o|seq)
end

% VB update of P(agent=kToM)
Pi = Pi0.*h_kToM./(Pi0.*h_kToM+(1-Pi0).*h_seq);
fx(inF.meta.indx) = VBA_sigmoid(Pi, 'inverse', true);

% 2- update k-ToM belief
inFktom.metaweight = Pi;
Par_ktom = P(inF.ktom.indP);
uktom = u(1:2);
fx(inF.ktom.indx) = RecToMfunction(xktom,Par_ktom,uktom,inFktom);

% 3- update seq learner belief
inFseq.metaweight = 1-Pi;
Par_seq = P(inF.seq.indP);
fx(inF.seq.indx) = f_BSL(xseq,Par_seq,useq,inFseq);


function b = fastnnls(x,y,tol,b)

% $ Version 1.02 $ Date 28. July 1998 $ Not compiled $
%
% See also:
% 'unimodal' 'monreg' 'fastnnls'
%
%  FASTNNLS Fast non-negative least squares
%  The inputs are the matrix of predictor variables (x),
%  vector of predicted variable (y), and optional inputs
%  tolerance on the size of a regression coefficient that is
%  considered zero (tol), and initial guess for the regression
%  vector (b0). The output is the non-negatively constrained
%  least squares solution (b).
%
%  If tol is set to 0, the default tolerance will be used.
%
%  FASTNNLS is fastest when a good estimate of the regression
%  vector is input. This eliminates much of the computation
%  involved in determining which coefficients will be nonzero
%  in the final regression vector. This makes it very useful
%  in alternating least squares routines. Note that the input
%  b0 must be a feasible (i.e. nonnegative) solution.
%
%  The FASTNNLS algorithm is based on the one developed by
%  Bro and de Jong, J. Chemometrics, Vol. 11, No. 5, 393-401, 1997
%
%I/O: b = fastnnls(x,y,tol,b0);
%
%See also: MCR, PARAFAC

% Copyright (C) 1995-2006  Rasmus Bro & Claus Andersson
% Copenhagen University, DK-1958 Frederiksberg, Denmark, rb@life.ku.dk
%
% 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.

[m,n] = size(x);
if (nargin < 3 | tol == 0)
  tol = max(size(x))*norm(x,1)*eps;
end
if nargin < 4
  b = zeros(n,1);
end

p = logical(zeros(1,n));
p(find(b>0)) = ones(size(find(b>0)));
r = ~p;

sp = x(:,p)\y;
b(find(p)) = sp;
while min(sp) < 0
  b(find(b<0)) = zeros(size(find(b<0)));
  p = logical(zeros(1,n));
  p(find(b>0)) = ones(size(find(b>0)));
  r = ~p;
  sp = x(:,p)\y;
  b(find(p)) = sp;
end

w = x'*(y-x*b);
[wmax,ind] = max(w);
flag = 0;
while (wmax > tol & any(r))
  p(ind) = 1;
  r(ind) = 0;
  sp = x(:,p)\y;
  while min(sp) < -tol
    tsp = zeros(n,1);
    tsp(find(p)) = sp;
    fb = find(b);
    rat = b(fb)./(eps+(b(fb)-tsp(fb)));
    alpha = min(rat(rat>tol));
    b = b + alpha*(tsp-b);
    p = b > tol;
    r = ~p;
    sp = x(:,p)\y;
  end
  b(find(p)) = sp;
  w = x'*(y-x*b);
  [wmax,ind] = max(w);
  if p(ind)
    wmax = 0;
  end
end


function [dx,dy]=   centered_gradient(input,dx,dy, nx,ny)
dx = zeros(1,round(nx*ny*+nx));
dy = zeros(1,round(nx*ny*+nx));
nx = round(nx);
ny = round(ny);
size(input);
for i = 1:ny-1
        for  j = 1:  nx-1
           k = round(i * nx + j);
           if(nx+k < length(input))
            dx(k) = 0.5*(input(k+1) - input(k-1));
            dy(k) = 0.5*(input(k+nx) - input(k-nx));
           end
        end
end
z = 1;
for  j = 2: nx-1

      dx(z) = 0.5*(input(z+1) - input(j-1));
        dy(z) = 0.5*(input(z+nx) - input(z));

    k = (ny - 1) * nx + z;
if(k < length(input))
        dx(k) = 0.5*(input(k+1) - input(k-1));
        dy(k) = 0.5*(input(k) - input(k-nx));
end
        z = z +1;
end
for  i = 1: ny-1
        p = (i * nx)+1;
        if(p+nx < length(input))
        dx(p) = 0.5*(input(p+1) - input(p));
        dy(p) = 0.5*(input(p+nx) - input(p-nx));
        end
        k = (i+1) * nx - 1;
if(k+nx < length(input))
        dx(k) = 0.5*(input(k) - input(k-1));
        dy(k) = 0.5*(input(k+nx) - input(k-nx));
end

    dx(1) = 0.5*(input(2) - input(1));
    dy(1) = 0.5*(input(nx) - input(1));

    dx(nx-1) = 0.5*(input(nx-1) - input(nx-2));
    dy(nx-1) = 0.5*(input(2*nx-1) - input(nx-1));

    dx((ny-1)*nx) = 0.5*(input((ny-1)*nx + 1) - input((ny-1)*nx));
    dy((ny-1)*nx) = 0.5*(input((ny-1)*nx) - input((ny-2)*nx));
    if(ny*nx-2 <length(input))
    dx(ny*nx-2) = 0.5*(input(ny*nx-2) - input(ny*nx-1-1-1));
    dy(ny*nx-2) = 0.5*(input(ny*nx-2) - input((ny-2)*nx-2));
    disp('at least once');
    end
    end
end


function [L,U,P] = lu_rightpr (A)
%LU_RIGHTPR recursive right-looking LU, with partial pivoting.
%
% Example:
%   [L,U,P] = lu_rightpr (A)
% See also: cs_demo

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

n = size (A,1) ;
if (n == 1)
    P = 1 ;
    L = 1 ;
    U = A ;
else
    [x,i] = max (abs (A (1:n,1))) ;                           % partial pivoting
    P1 = eye (n) ;
    P1 ([1 i],:) = P1 ([i 1], :) ;
    A = P1*A ;
    u11 = A (1,1) ;                                           % (6.10)
    u12 = A (1,2:n) ;                                         % (6.11)
    l21 = A (2:n,1) / u11 ;                                   % (6.12)
    [L22,U22,P2] = lu_rightpr (A (2:n,2:n) - l21*u12) ;       % (6.9) or (6.13)
    o = zeros(1,n-1) ;
    L = [ 1 o ; P2*l21 L22 ] ;                                % (6.14)
    U = [ u11 u12 ; o' U22 ] ;
    P = [ 1 o ; o' P2] * P1 ;
end


function errSS = dtiComputeCurveDiff(param,origCurve,targetCurve)
% Compute error sum of squares between 2 curves given the warping params
%
%    errSS = dtiComputeCurveDiff(param,origCurve,targetCurve)
%
%    param       - 4x1 vector [xScale xShift yScale yShift]
%    origCurve   - n1x2 matrix [origX origY]
%    targetCurve - n2x2 matrix [targetX targetY]
%
%    errSS - sum of squares of differences between the 2 curves
%
% History:
%    2007/01/17 shc wrote it.
%

if ieNotDefined('param'),       error('Require warping parameters!');  end
if ieNotDefined('origCurve'),   error('Require original curve data!'); end
if ieNotDefined('targetCurve'), error('Require target curve data!');   end

x1 = origCurve(:,1);
x2 = targetCurve(:,1);

startPt = max([min(x1) dtiUnwarpStep(min(x2),param(1:2))]);
endPt   = min([max(x1) dtiUnwarpStep(max(x2),param(1:2))]);

xs = linspace(startPt,endPt,5000);

y1Hat = (spline(origCurve(:,1),origCurve(:,2),xs) + param(4)) * param(3);
y2Hat = spline(targetCurve(:,1),targetCurve(:,2),dtiWarpStep(xs,param));

errSS = sum((y1Hat - y2Hat) .^ 2) / (endPt - startPt);

return


function sp = conventional_bf_1d(R, n, design, wavelength, grid_size, varargin)
%CONVENTIONAL_BF_1D 1D conventional (Bartlett) beamforming.
%Syntax:
%   sp = CONVENTIONAL_BF_1D(R, n, design, wavelength, grid_size, ...);
%   sp = CONVENTIONAL_BF_1D(R, n, f_steering, [], grid_size, ...);
%Inputs:
%   R - Sample covariance matrix.
%   n - Number of sources.
%   design - Array design. Can also be a function handle that generates
%            a steering matrix. This function must take two arguments,
%            wavelength and the doa vector.
%   wavelength - Wavelength.
%   grid_size - Number of grid points used.
%   ... - Options:
%           'Unit' - Can be 'radian', 'degree', or 'sin'. Default value is
%                   'radian'.
%           'RefineEstimates' - If set to true, will refine the estimated
%                               direction of arrivals around the grid.
%Output:
%   sp - Spectrum structure with the following fields:
%           x - An 1 x grid_size vector.
%           y - An 1 x grid_size vector. Calling `plot(x, y)` will plot the
%               spectrum.
%           x_est - An 1 x n vector storing the estimated DOAs. May not
%                   fall on the grid if 'RefineEstimates' is set to true.
%           x_unit - The same as the unit specified by 'Unit'.
%           resolved - True if the number of peaks in the spectrum is
%                      greater or equal to the number of sources.
%           discrete - Constant value false.
%Reference:
%   [1] H. L. Van Trees, Optimum array processing. New York: Wiley, 2002.
unit = 'radian';
refine_estimates = false;
for ii = 1:2:nargin-5
    option_name = varargin{ii};
    option_value = varargin{ii+1};
    switch lower(option_name)
        case 'unit'
            unit = lower(option_value);
        case 'refineestimates'
            refine_estimates = true;
        otherwise
            error('Unknown option ''%s''.', option_name);
    end
end
% discretize and create the corresponding steering matrix
[doa_grid_rad, doa_grid_display, ~] = default_doa_grid(grid_size, unit, 1);
% compute spectrum
sp_intl = compute_spectrum(R, design, wavelength, doa_grid_rad);
[x_est, x_est_idx, resolved] = find_doa_from_spectrum_1d(doa_grid_display, sp_intl, n);
% refine
if resolved && refine_estimates
    switch unit
        case 'radian'
            f_obj = @(x) compute_spectrum(R, design, wavelength, x);
        case 'degree'
            f_obj = @(x) compute_spectrum(R, design, wavelength, deg2rad(x));
        case 'sin'
            f_obj = @(x) compute_spectrum(R, design, wavelength, asin(x));
        otherwise
            error('Invalid unit ''%s''.', unit);
    end
    x_est = refine_grid_estimates(f_obj, doa_grid_rad, x_est_idx);
end
% return
sp = struct();
sp.x = doa_grid_display;
sp.x_est = x_est;
sp.x_unit = unit;
sp.y = sp_intl;
sp.resolved = resolved;
sp.discrete = false;
end

function v = compute_spectrum(R, design, wavelength, theta)
if ishandle(design)
    A = design(wavelength, theta);
else
    A = steering_matrix(design, wavelength, theta);
end
v = real(sum(conj(A) .* (R * A), 1));
end


function mtrPlotLengthPenalty(length_absorption)
%
% mtrPlotLengthPenalty([length_absorption=0.8])
%

if(~exist('length_absorption','var')|isempty(length_absorption))
    length_absorption = 0.800;
end

FA = [0.88, 0.7664, 0.50, 0.30, 0.15];
cor = {'k', 'g', 'r', 'm', 'c'};
stdev = 1.0978*exp(-1.9567*FA) - 0.1437;
%maxprior = 1/(2*pi)*normpdf(0,0,0.628)/(normcdf(pi,0,0.628)-0.5);
maxprior = normpdf(0,0,0.628)/(normcdf(pi,0,0.628)-0.5);
maxlike = normpdf(0,0,stdev)./(normcdf(pi/2,0,stdev)-0.5);
maxcortexlike = normpdf(0,0,0.15)./(normcdf(pi/2,0,0.15)-0.5);
%length_penalty = pi/5;
length_penalty = 1-length_absorption;

x = [50:2:250];
for i = 1:length(maxlike)
    log_segment_score = log(maxprior * maxlike(i) * length_penalty);
    y = (x-4)/2*log_segment_score + 2*log(0.999)*maxprior*maxcortexlike;
    plot(x,y,cor{i},'LineWidth',2)
    hold on;
end
hold off;

%legend('STT Path','FA = 0.88','FA = 0.77','FA = 0.50','FA = 0.30','FA = 0.15');
legend('FA = 0.88','FA = 0.77','FA = 0.50','FA = 0.30','FA = 0.15');
% Find where stddev will make the function not penalize or reward
stdsearch = [0:0.01:pi/2];
maxlike = normpdf(0,0,stdsearch)./(normcdf(pi/2,0,stdsearch)-0.5);
abs_post = abs(maxprior*maxlike*length_penalty-1);
[junk, mi] = min(abs_post);
stdev_zero = stdsearch(mi);
fa_zero = log((stdev_zero+0.1437)/1.0978)/(-1.9567)


function X = QPbyUseSFC(waypts, ts, decomp)

%% condition
traj.n_order = 7;
traj.n_poly = size(waypts, 1) - 1;
traj.p0 = waypts(1,:);
traj.pe = waypts(end,:);
traj.v0 = [0,0,0];
traj.ve = [0,0,0];
traj.a0 = [0,0,0];
traj.ae = [0,0,0];

%% trajectory plan
[minSnapValue, px, py, pz] = minimum_snap_three_axis_SFC(traj, ts, decomp);

disp(['minSnapValue is : ',num2str(minSnapValue)]);

X = [px py pz];
end

% v0 = [1*3]
function [minValue, px, py, pz] = minimum_snap_three_axis_SFC(traj, ts, decomp)
    % Dimension
    dim = 3;
    n_order = traj.n_order;
	p0 = traj.p0;
	pe = traj.pe;
    v0 = traj.v0;
    ve = traj.ve;
    a0 = traj.a0;
    ae = traj.ae;

	n_poly = traj.n_poly;
	n_coef = n_order+1;

	%% compute Q,  Q is a Symmetric matrix
    % Q_1 =   [0    0   0]
    %         [0    f   f]
    %         [0    f   f]          % f =  integral of ts(i) ~ ts(i+1)
    % Q_x =   [Q_1  0     0    0]
    %         [0    Q_2   0    0]
    %         [0    0   Q_...  0]
    %         [0    0     0  Q_n]   % n = n_poly
    % Q_all = [Q_x  0     0]
    %         [0    Q_y   0]
    %         [0    0   Q_z]        % Q_x == Q_y == Q_z
    Q_x = [];
	for i=1:n_poly
	    Q_x = blkdiag(Q_x,calc_Q(n_order,4,ts(i),ts(i+1)));
    end
    zeroM = zeros(size(Q_x));
    Q_all = [Q_x zeroM zeroM; zeroM Q_x zeroM; zeroM zeroM Q_x];
    xlen = size(Q_all,1);
	f = zeros(xlen,1);      %% min (1/2p^TQp + f^Tp)

    %% compute Aeq x = beq
    % beacuse Aeq_x == Aeq_y == Aeq_z
    % Aeq = [Aeq_x  0      0]
    %       [0    Aeq_y    0]
    %       [0      0  Aeq_z]
	neq = 8;  %% (8 equations)
    eqNum_x = (7*(n_poly-1)+neq);
	Aeq_x = zeros(eqNum_x, n_coef*n_poly);
	beq = zeros(dim*eqNum_x, 1);

	% start/terminal pva constraints  (8 equations)
	Aeq_x(1:4,1:n_coef) = [calc_dc(ts(1),n_order,0);
	                     calc_dc(ts(1),n_order,1);
	                     calc_dc(ts(1),n_order,2);
	                     calc_dc(ts(1),n_order,3)];
	Aeq_x(5:8,n_coef*(n_poly-1)+1:n_coef*n_poly) = ...
	                    [calc_dc(ts(end),n_order,0);
	                     calc_dc(ts(end),n_order,1);
	                     calc_dc(ts(end),n_order,2);
	                     calc_dc(ts(end),n_order,3)];
	beq(1:8,1)                              = [p0(1),v0(1),a0(1),0,pe(1),ve(1),ae(1),0]';
    beq((eqNum_x   + 1):(eqNum_x   + 8),1)  = [p0(2),v0(2),a0(2),0,pe(2),ve(2),ae(2),0]';
    beq((eqNum_x*2 + 1):(eqNum_x*2 + 8),1)  = [p0(3),v0(3),a0(3),0,pe(3),ve(3),ae(3),0]';

	% continuous constraints  ((n_poly-1)*7 equations)
	for i=1:n_poly-1
		t_derc_p = calc_dc(ts(i+1),n_order,0);
        t_derc_v = calc_dc(ts(i+1),n_order,1);
        t_derc_a = calc_dc(ts(i+1),n_order,2);
        t_derc_j = calc_dc(ts(i+1),n_order,3);  % jerk
        t_derc_4 = calc_dc(ts(i+1),n_order,4);
        t_derc_5 = calc_dc(ts(i+1),n_order,5);
        t_derc_6 = calc_dc(ts(i+1),n_order,6);
        Aeq_x(neq+1,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_p,-t_derc_p];
        Aeq_x(neq+2,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_v,-t_derc_v];
        Aeq_x(neq+3,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_a,-t_derc_a];
        Aeq_x(neq+4,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_j,-t_derc_j];
        Aeq_x(neq+5,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_4,-t_derc_4];
        Aeq_x(neq+6,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_5,-t_derc_5];
        Aeq_x(neq+7,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_6,-t_derc_6];
        neq = neq + 7;
    end

    zeroM = zeros(size(Aeq_x));
    Aeq = [Aeq_x zeroM zeroM; zeroM Aeq_x zeroM; zeroM zeroM Aeq_x];

    %% compute Ax <= b
    A = [];
    b = [];
    ieq = 1;
    xll = xlen / dim;  % 1/3 of xlen,  = n_coef * (n_poly-1)
    for i = 1 : n_poly
        planesCell = decomp.lines_{i}.polyhedron_.polys_;
        [~, lenj] = size(planesCell);
        for j = 1 : lenj
            n = planesCell{j}.n_;
            p = planesCell{j}.p_;
            nx = zeros(1, xlen);
            % add (Denominator + 1) points on the poly's Ax<b
            Denominator = 2;  % Denominator > 0
            for k = 0 : Denominator
                tvec_mid = calc_dc(ts(i) +  k*(ts(i+1) - ts(i))/Denominator, n_order, 0);   % [1*8]vector
                nx(1, 8*(i-1) + 1 : 8*(i-1) + 8)                = tvec_mid.*n(1);
                nx(1, 8*(i-1) + 1 + xll : 8*(i-1) + 8 + xll)    = tvec_mid.*n(2);
                nx(1, 8*(i-1) + 1 + 2*xll : 8*(i-1) + 8 + 2*xll)= tvec_mid.*n(3);
                A(ieq,:) = nx;
                b(ieq,:) = dot(n, p);
                % dot(plane.n, p1 - plane.p) < 0
                % => dot(plane.n, p1) - dot(plane.n, plane.p) < 0
                % => dot(plane.n, p1) < dot(plane.n, plane.p)
                %% Ax < b
                ieq = ieq + 1;
            end
        end
    end

%     % Modify the number of iterations
    options = optimoptions('fmincon');
    options.MaxIterations = 2000;   % Defaults = 1000
    lb = []; ub = []; x0 = [];
    p = quadprog(Q_all,f,A,b,Aeq,beq, lb,ub,x0, options);

% 	p = quadprog(Q_all,f,A,b,Aeq,beq);

    minValue = p'*Q_all*p;

    px = p(1:xll,1);
    py = p(xll + 1:2*xll,1);
    pz = p(xll*2 + 1:3*xll,1);
end


% set values, load data
nx = 144;
ny = 256;
nc = 8;
niter = 250;
mu = 2^-3;
nlevels = 3;
beta = 2^8.5;

load braindat;
load kmask;
load brainxinf;
% load braindb4xinf;
mask = true(nx,ny);

% mask = abs(xinf) > 0.1*max(col(abs(xinf)));

% generate and sample data
A = (1/sqrt(nx*ny))*Gdft('mask', true(nx,ny), 'fftshift', 1, ...
    'ifftshift', 1);

dat = A*reshape(coilim, [nx*ny nc]); clear coilim;

kmask = kmask((256-nx)/2+1:256-(256-nx)/2,:);
dat = col(dat(col(kmask),:));

n = kmask; b = 1:nx*ny;
n = b(n); clear b;

% build system matrices
A = Apsm('knownfn', 'time', 'v', 1, 'n', n(:), 'smap', smap, 'immask', ...
    true(nx,ny), 'nk', nx*ny);
W = Godwt1(mask, 'level', nlevels, 'wname', 'haar');
P = Gdiag(1./(col(sum(abs(smap).^2,3)) + mu));

% don't regularize approx coeffs
beta = beta*ones(nx,ny);
beta(1:nx/2^nlevels,1:ny/2^nlevels) = 0;
beta = col(beta);

% initializations
x = A'*dat; x = x./ col(sum(abs(smap).^2,3));
x = x(mask);
z = W*x;
eta = zeros(size(z));

% system matrix
A = Apsm('knownfn', 'time', 'v', 1, 'n', n(:), 'smap', smap, 'immask', ...
    mask, 'nk', nx*ny);

% algorithm book-keeping
shrink = @(t, a) (t-a .* sign(t)) .* (abs(t) > a);
i = 0;
thetime(1) = 0;
xdist(1) = norm(col(x) - col(xinf(mask)));

% go
tic;
while i < niter
    printm('iteration %d of %d', i+1, niter);

    z = shrink(W*x + eta, beta./mu);
    x = qpwls_pcg1(x, [A; sqrt(mu)*W], 1, [dat(:); sqrt(mu)*(z - eta)], ...
        0, 'niter', 5, 'precon', P);
    eta = eta - (z - W*x);

    thetime(i+2) = toc;
    xdist(i+2) = norm(col(x) - col(xinf(mask)));
    i = i+1;
end
toc;
x = embed(x, mask);
im(x)


function ref = buildRefTraj(q,dq,ddq,ref)
% ref = buildRefTraj(q,dq,ddq,ref)
%
% This function computes the piecewise polynomial trajectories that make up
% the reference trajectory for the feedback linearization
%
% INPUTS:
%   q = [nConfig, nTime] = reference trajectory in configuration space
%   dq = [nConfig, nTime] = dq/dt = rates in time for ref traj
%   ddq = [nConfig, nTime] = ddq/ddt = accelerations for ref traj
%   ref = prototype for ref struct, with fields:
%       .wn = natural frequency of the controller
%       .xi = damping ratio of the controller
%       .c = [1, nConfig] = mapping from configuration to phase
%       .H = [nMeasure, nConfig] = mapping from configuration to measurement
%
% OUTPUTS:
%
%   ref = full ref struct, with added fields:
%       .pp = piecewise-polynomial trajectories for:
%           .h = reference measurement vector
%           .dh = dMeasurement/dPhase
%           .ddh = second derivative of measurements with respect to phase
%           .dhdt = dMeasurement/dTime
%
%

% Trajectories: measurement and phase vs time:
hRef = ref.H*q;   % Target measurement
dhRef = ref.H*dq;
ddhRef = ref.H*ddq;
pRef = ref.c*q; % Phase
dpRef = ref.c*dq;
ddpRef = ref.c*ddq;

% Compute derivatives wrt phase using chain rule:
dhRefdp = dhRef./dpRef;
ddhRefddp = (ddhRef - dhRefdp.*ddpRef)./(dpRef.^2);

% Represent trajectories as piecewise-polynomial
ref.pp.h = pchip(pRef,hRef);
ref.pp.dh = pchip(pRef,dhRefdp);   % Derivative wrt phase  (for ref traj)
ref.pp.dhdt = pchip(pRef,dhRef);   % Derivative wrt time   (for stabilization)
ref.pp.ddh = pchip(pRef,ddhRefddp);

end


%IMM_PREDICT  Interacting Multiple Model (IMM) Filter prediction step
%
% Syntax:
%   [X_p,P_p,c_j,X,P] = IMM_PREDICT(X_ip,P_ip,MU_ip,p_ij,ind,dims,A,Q)
%
% In:
%   X_ip  - Cell array containing N^j x 1 mean state estimate vector for
%           each model j after update step of previous time step
%   P_ip  - Cell array containing N^j x N^j state covariance matrix for
%           each model j after update step of previous time step
%   MU_ip - Vector containing the model probabilities at previous time step
%   p_ij  - Model transition probability matrix
%   ind   - Indexes of state components for each model as a cell array
%   dims  - Total number of different state components in the combined system
%   A     - State transition matrices for each model as a cell array.
%   Q     - Process noise matrices for each model as a cell array.
%
% Out:
%   X_p   - Predicted state mean for each model as a cell array
%   P_p   - Predicted state covariance for each model as a cell array
%   c_j   - Normalizing factors for mixing probabilities
%   X     - Combined predicted state mean estimate
%   P     - Combined predicted state covariance estimate
%
% Description:
%   IMM filter prediction step.
%
% See also:
%   IMM_UPDATE, IMM_SMOOTH, IMM_FILTER

% History:
%   01.11.2007 JH The first official version.
%
% Copyright (C) 2007 Jouni Hartikainen
%
% $Id: imm_update.m 111 2007-11-01 12:09:23Z jmjharti $
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.

function [X_p,P_p,c_j,X,P] = imm_predict(X_ip,P_ip,MU_ip,p_ij,ind,dims,A,Q)
    % Number of models
    m = length(X_ip);

    % Default values for state mean and covariance
    MM_def = zeros(dims,1);
    PP_def = diag(20*ones(dims,1));

    % Normalizing factors for mixing probabilities
    c_j = zeros(1,m);
    for j = 1:m
        for i = 1:m
            c_j(j) = c_j(j) + p_ij(i,j).*MU_ip(i);
        end
    end

    % Mixing probabilities
    MU_ij = zeros(m,m);
    for i = 1:m
        for j = 1:m
            MU_ij(i,j) = p_ij(i,j) * MU_ip(i) / c_j(j);
        end
    end

    % Calculate the mixed state mean for each filter
    X_0j = cell(1,m);
    for j = 1:m
        X_0j{j} = zeros(dims,1);
        for i = 1:m
            X_0j{j}(ind{i}) = X_0j{j}(ind{i}) + X_ip{i}*MU_ij(i,j);
        end
    end

    % Calculate the mixed state covariance for each filter
    P_0j = cell(1,m);
    for j = 1:m
        P_0j{j} = zeros(dims,dims);
        for i = 1:m
            P_0j{j}(ind{i},ind{i}) = P_0j{j}(ind{i},ind{i}) + MU_ij(i,j)*(P_ip{i} + (X_ip{i}-X_0j{j}(ind{i}))*(X_ip{i}-X_0j{j}(ind{i}))');
        end
    end

    % Space for predictions
    X_p = cell(1,m);
    P_p = cell(1,m);

    % Make predictions for each model
    for i = 1:m
        [X_p{i}, P_p{i}] = kf_predict(X_0j{i}(ind{i}),P_0j{i}(ind{i},ind{i}),A{i},Q{i});
    end

    % Output the combined predicted state mean and covariance, if wanted.
    if nargout > 3
        % Space for estimates
        X = zeros(dims,1);
        P = zeros(dims,dims);

        % Predicted state mean
        for i = 1:m
            X(ind{i}) = X(ind{i}) + MU_ip(i)*X_p{i};
        end

        % Predicted state covariance
        for i = 1:m
            P(ind{i},ind{i}) = P(ind{i},ind{i}) + MU_ip(i)*(P_p{i} + (X_ip{i}-X(ind{i}))*(X_ip{i}-X(ind{i}))');
        end
    end


% PROPAGATE A GAUSSIAN ERROR DISTRIBUTION OF EACH JOINT TO AN ERROR IN
% END EFFECTORS' POSITION
% AN ELLIPSE IS DRAWN TO REPRESENT THE ERROR IN POSITION

% 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/>.

%load arm parameters
robot= load_robot('example','scara')

%find errors around pose
q=[pi/4 3*pi/4 0 0];

%Matriz de errores en las articulaciones
% sigmaq1=sigmaq2=0.0017 rad, sigmaq3=0.01 m.
sigmaq1=0.017;%rad, 0.01 grados
sigmaq2=0.017;%rad
sigmaq3=0.01;% m
Rq=[sigmaq1^2 0 0;
    0 sigmaq2^2 0;
    0 0 sigmaq3^2]

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

Jq = eval(robot.J)

Rp=Jq*Rq*Jq'

T=directkinematic(robot, q);

drawrobot3d(robot,q), hold on
draw_ellipse([T(1,4),T(2,4)], Rp, 'r')


function sp = esprit_1d(R, n, k, varargin)
%ESPRIT_1D 1D ESPRIT for ULAs.
%Syntax:
%   sp = ESPRIT_1D(R, n, design, wavelength, grid_size, ...);
%Inputs:
%   R - Sample covariance matrix.
%   n - Number of sources.
%   k - 2*pi*inter_element_spacing/wavelength.
%   ... - Options:
%           'Unit' - Can be 'radian', 'degree', or 'sin'. Default value is
%                   'radian'.
%           'Displacement' - The displacement between the two overlapping
%                            subarrays measured in number of element
%                            spacings. Default value is 1.
%                            Note: increasing this value will lead to
%                            smaller unambiguous range. Make sure your DOAs
%                            falls within the unambiguous range.
%           'Formulation' - Either 'TLS' (Total Lease Squares) or 'LS'
%                           (Least Squares). Default value is 'TLS'.
%           'RowWeights' - Specifies the row weights with a vector or
%                          string. Default value is 'Default', which
%                          generates the following weight vector:
%                           [1 sqrt(2) sqrt(3) ... sqrt(3) sqrt(2) 1]
%                          You can disable row weighting by passing in
%                          'Identity' or 'Off'.
%Output:
%   sp - Spectrum structure with the following fields:
%           x - An 1 x grid_size vector.
%           y - An 1 x grid_size vector. Calling `plot(x, y)` will plot the
%               spectrum.
%           x_est - An 1 x n vector storing the estimated DOAs.
%           x_unit - The same as the unit specified by 'Unit'.
%           resolved - Constant value true.
%           discrete - Constant value true.
%Reference:
%   [1] H. L. Van Trees, Optimum array processing. New York: Wiley, 2002.
use_tls = true;
ds = 1;
row_weights = [];
use_row_weights = false;
unit = 'radian';
for ii = 1:2:nargin-3
    option_name = varargin{ii};
    option_value = varargin{ii+1};
    switch lower(option_name)
        case 'unit'
            unit = option_value;
        case 'formulation'
            switch lower(option_value)
                case 'ls'
                    use_tls = false;
                case 'tls'
                    use_tls = true;
                otherwise
                    error('Formulation must be either ''LS'' or ''TLS''.');
            end
        case 'displacement'
            if option_value < 1 || mod(option_value, 1) ~= 0
                error('Displacement must be an integer that is greater or equal to one.');
            end
            ds = option_value;
        case 'rowweights'
            if ischar(option_value)
                switch lower(option_value)
                    case 'default'
                        use_row_weights = true;
                    case {'off', 'identity'}
                        use_row_weights = false;
                    otherwise
                        error('Either specify the row weights manually or pass in ''Default'' to use the default weights, or ''Identity'', ''Off'' to disable row weighting.');
                end
            else
                use_row_weights = true;
                row_weights = option_value(:);
            end
        otherwise
            error('Unknow option "%s".', option_name);
    end
end
m = size(R, 1);
if n > m - ds
    error('Too many sources.');
end
if ~isempty(row_weights) && length(row_weights) ~= m - ds
    error('The dimension of the row weights vector is not equal to (m - displacement).');
end
% ESPRIT
[E, ~] = eig(0.5*(R + R'), 'vector');
Es = E(:,end - n + 1:end);
% apply weights if necessary
if use_row_weights
    if isempty(row_weights)
        % default weights
        if mod(m - ds, 2) == 1
            w_max = (m - ds - 1)/2;
            row_weights = sqrt([1:w_max w_max + 1 w_max:-1:1])';
        else
            w_max = (m - ds)/2;
            row_weights = sqrt([1:w_max w_max:-1:1])';
        end
    end
    Es1 = bsxfun(@times, row_weights, Es(1:end - ds,:));
    Es2 = bsxfun(@times, row_weights, Es(ds + 1:end,:));
else
    Es1 = Es(1:end - ds,:);
    Es2 = Es(ds + 1:end,:);
end
if use_tls
    % TLS estimate
    C = [Es1 Es2];
    C = C'*C;
    C = 0.5*(C + C');
    [V, l] = eig(C, 'vector');
    [~, idx] = sort(real(l), 'descend');
    V = V(:,idx);
    V12 = V(1:n,n + 1:end);
    V22 = V(n + 1:end,n + 1:end);
    Phi = -V12/V22;
else
    % LS estimate
    Phi = (Es1'*Es1)\(Es1'*Es2);
end
% convert z to spectrum
z = eig(Phi);
sp = struct();
sp.x_est = sort(cm2doa(z, k*ds, unit));
sp.x = sp.x_est;
sp.x_unit = unit;
sp.y = ones(1, n);
sp.resolved = true;
sp.discrete = true;
end


function [F, M, trpy, drpy] = controller(qd, t, qn, params)
% CONTROLLER quadrotor controller
% The current states are:
% qd{qn}.pos, qd{qn}.vel, qd{qn}.euler = [roll;pitch;yaw], qd{qn}.omega
% The desired states are:
% qd{qn}.pos_des, qd{qn}.vel_des, qd{qn}.acc_des, qd{qn}.yaw_des, qd{qn}.yawdot_des
% Using these current and desired states, you have to compute the desired controls

% =================== Your code goes here ===================

% ordinary linear
% % position controller params
% Kp = ones(3,1)*30;
% Kd = ones(3,1)*10;
%
% % attitude controller params
% KpM = ones(3,1)*10000;
% KdM = ones(3,1)*500;

% position controller params
Kp = [15;15;30];
% Kd = [15;15;10];
Kd = [12;12;10];

% attitude controller params
KpM = ones(3,1)*3000;
KdM = ones(3,1)*300;

% t = qd{qn}.vel_des/norm(qd{qn}.vel_des+eps);
% n = qd{qn}.acc_des/norm(qd{qn}.acc_des+eps);
% b = cross(t,n);
% ep = ((qd{qn}.pos_des - qd{qn}.pos).*n).*n + ((qd{qn}.pos_des - qd{qn}.pos).*b).*b;
% ev = qd{qn}.vel_des - qd{qn}.vel;

acc_des = qd{qn}.acc_des + Kd.*(qd{qn}.vel_des - qd{qn}.vel) + Kp.*(qd{qn}.pos_des - qd{qn}.pos);
% acc_des = qd{qn}.acc_des + Kd.*ev + Kp.*ep

% Desired roll, pitch and yaw
phi_des = 1/params.grav * (acc_des(1)*sin(qd{qn}.yaw_des) - acc_des(2)*cos(qd{qn}.yaw_des));
theta_des = 1/params.grav * (acc_des(1)*cos(qd{qn}.yaw_des) + acc_des(2)*sin(qd{qn}.yaw_des));
psi_des = qd{qn}.yaw_des;

euler_des = [phi_des;theta_des;psi_des];
pqr_des = [0;0; qd{qn}.yawdot_des];
% Thurst
qd{qn}.acc_des(3);
F  = params.mass*(params.grav + acc_des(3));
% Moment
M =  params.I*(KdM.*(pqr_des - qd{qn}.omega) + KpM.*(euler_des - qd{qn}.euler));
% =================== Your code ends here ===================

% Output trpy and drpy as in hardware
trpy = [F, phi_des, theta_des, psi_des];
drpy = [0, 0,       0,         0];

end


function cplxPot
% complex potential flow
%    using MATLAB analytical solutions
%
%   $Ekkehard Holzbecher  $Date: 2006/05/31 $
%--------------------------------------------------------------------------
% Baseflow
H = 10;             % thickness [L]
h0 = 5;             % reference piezometric head [L]
K = 5.e-5;          % hydraulic conductivity [L/T]
Qx0 = 0;            % baseflow in x-direction [L^2/T]
Qy0 = 0;            % baseflow in y-direction [L^2/T]

% Wells
xwell = [150 250];          % x-coordinates well position [L]
ywell = [0 0];              % y-coordinates well position [L]
Qwell = 1.e-4*[1 -1];       % pumping / recharge rates [L^3/T]
R = [1 1];                  % well radius [L]

% Mesh
xmin = 0;           % minimum x-position of mesh [L]
xmax = 400;         % maximum x-position of mesh [L]
ymin = -100;        % minimum y-position of mesh [L]
ymax = 100;         % maximum y-position of mesh [L]

% Reference point position in mesh
iref = 1; jref = 1;

% Graphical output options
gsurfh = 0;         % piezometric head surface plot
gcontf = 16;        % no. filled contour lines (=0: none)
gquiv = 0;          % arrow field plot
gflowp_fit = 0;     % flowpaths forward in time
gflowp_bit = 0;     % no. flowpaths backward in time (=0: none)
gflowp_dot = 0;     % flowpaths with dots indicating speed
gstream = 10;       % streamfunction plot

%----------------------------------------execution-------------------------------
xvec = linspace(xmin,xmax,50);
yvec = linspace(ymin,ymax,50);
[x,y] = meshgrid (xvec,yvec);                      % mesh

phi = -Qx0*x - Qy0*y;
psi = -Qx0*y + Qy0*x;
for i = 1:size(xwell,2)
    r = sqrt((x-xwell(i)).*(x-xwell(i))+(y-ywell(i)).*(y-ywell(i)));
    phi = phi + (Qwell(i)/(2*pi))*log(r);   % potential
    psi = psi + (Qwell(i)/(2*pi))*atan2((y-ywell(i)),(x-xwell(i)));
end
if h0 > H
    phi0 = -phi(iref,jref) + K*H*h0 - 0.5*K*H*H;
else
    phi0 = -phi(iref,jref) + 0.5*K*h0*h0;       % reference potential
end
hc = 0.5*H+(1/K/H)*(phi+phi0);                  % head confined
hu = sqrt ((2/K)*(phi+phi0));                   % head unconfined
phicrit = phi0 + 0.5*K*H*H;                     % transition confined / unconfined
confined = (phi>=phicrit);                      % confined / unconfined indicator
h = confined.*hc+~confined.*hu;                 % head

%---------------------------------------display messages-------------------
if all(all(confined))
    display ('aquifer confined');
else
    if all(all(~confined))
        display ('aquifer unconfined');
    else
        display ('aquifer partially confined and unconfined');
    end
end
if any(any(h<0))
    display ('aquifer falls partially dry');
    h = max(0, h);
end
[u,v] = gradient (-phi);

%--------------------------------------graphical output--------------------
if gsurfh
    figure; surf (x,y,h);                             % surface
end
figure;
if gcontf                                             % filled contours
    colormap(winter);
    contourf (x,y,h,linspace(5-max(max(h-5)),5+max(max(h-5)),gcontf),'w');
    colorbar; hold on;
end
if gquiv
    quiver (x,y,u,v,'y'); hold on;                    % arrow field
end
if gflowp_fit                                         % flowpaths
    xstart = []; ystart = [];
    for i = 1:100
        if v(1,i) > 0 xstart = [xstart xvec(i)];...
                ystart = [ystart yvec(1)]; end
        if v(100,i) < 0 xstart = [xstart xvec(i)];...
                ystart = [ystart yvec(100)]; end
        if u(i,1) > 0 xstart = [xstart xvec(1)];...
                ystart = [ystart yvec(i)]; end
        if u(i,100) < 0 xstart = [xstart xvec(100)];...
                ystart = [ystart yvec(i)]; end
    end
    h = streamline (x,y,u,v,xstart,ystart);
    set (h,'Color','r');
end
if gflowp_bit
    xstart = x0 + R*cos(2*pi*[1:1:gflowp_bit]/gflowp_bit);
    ystart = y0 + R*sin(2*pi*[1:1:gflowp_bit]/gflowp_bit);
    h = streamline (x,y,-u,-v,xstart,ystart);
    set (h,'Color','y')
end
if gflowp_dot
    [verts averts] = streamslice(x,y,u,v,gflowp_dot);
    sc = 10/mean(mean(sqrt(u.*u+v.*v)));
    iverts = interpstreamspeed(x,y,u,v,verts,sc);
    h = streamline(iverts);
    set (h,'Marker','.','Color','y','MarkerSize',18)
end
if gstream
    h = contour (x,y,psi,gstream,'k','LineWidth',1);
end


function cost = computeBoundariesCost(opts, X, theta_x, theta_y, theta_z, T)
    H = eye(4);
    H(1:3,1:3) = rotx(theta_x) * roty(theta_y) * rotz(theta_z);
    H(1:3,4) = T';
    X_transformed = H * X;
    cost_x_pos = sum(X_transformed(1, X_transformed(1,:)>0));
    cost_x_neg = sum(X_transformed(1, X_transformed(1,:)<0));

    cost_y_pos = sum(X_transformed(2, X_transformed(2,:)>0));
    cost_y_neg = sum(X_transformed(2, X_transformed(2,:)<0));

    cost_z_pos = sum(X_transformed(3, X_transformed(3,:)>0));
    cost_z_neg = sum(X_transformed(3, X_transformed(3,:)<0));
    figure(1)
    hold on
    scatter3(X_transformed(1,:), X_transformed(2,:), X_transformed(3,:), 'r.')
end


% File testLUSOL.m
%
% Script for testing various LUSOL factorizations
% on the merged_S.mat matrix from 5 Dec 2006
% (S = 62177 x 75644 916437).
%
% 25 Jan 2008: First experiments on this particular S.
%              All used options.FactorTol = 2.
%              S is badly scaled, but several options
%              find the rank to be 61833 (rank deficiency 344).

load merged_S
[m,n] = size(S)
nnzS  = nnz(S)

options = lusolSet;
options.FactorTol = 2.0;
options.Pivoting = 'TPP';

%-----------------------------------------------------------------------------
% Try cheapest method:  L*U = S'
% It returns rank 61833.
disp(' ')
disp('Factor S(transpose)')
ST = S';
tic
[L1,U1,p1,q1,options] = lusolFactor(ST,options);
toc

% m       75644 >n       62177  Elems   916437  Amax   8.0E+05  Density   0.02
% Singular(m>n)  rank    61833  n-rank     344  nsing      344
% Merit    95.3  lenL   306685  L+U    1051242  Cmpressns    0  Incres   14.71
% Utri      556  lenU   744557  Ltol  2.00E+00  Umax   8.0E+05  Ugrwth 1.0E+00
% Ltri     3633  dense1      0  Lmax  2.00E+00
% bump    71455  dense2      0  DUmax  2.5E+03  DUmin  5.0E-01  condU  5.0E+03

rank1 = options.Rank;
rows1 = q1(1:rank1);
S1    = S(rows1,:);  % These should be independent rows of S.

%-----------------------------------------------------------------------------
% Try Rook Pivoting.
% S is too badly scaled for this to be efficient.
% Find column and row scales first.
disp(' ')
disp('Scale S now')
iprint  = 1;
scltol  = 0.9;
tic
[cscale,rscale] = gmscal(S,iprint,scltol);
disp(' ')
toc

% Apply scale factors to S.
C = spdiags(cscale,0,n,n);   Cinv = spdiags(1./cscale,0,n,n);
R = spdiags(rscale,0,m,m);   Rinv = spdiags(1./rscale,0,m,m);
SS = Rinv*S*Cinv;  % Scaled S
%-----------------------------------------------------------------------------

% Factor scaled SS with rook pivoting.
% It returns rank 61833.
disp(' ')
disp('Factor scaled S now with rook pivoting')
options.Pivoting = 'TRP';
tic
[L2,U2,p2,q2,options] = lusolFactor(SS,options);
toc

% m       62177 <n       75644  Elems   916437  Amax   1.0E+00  Density   0.02
% Singular(m<n)  rank    61833  n-rank   13811  nsing    13811
% MerRP    39.5  lenL   210899  L+U    1617359  Cmpressns    5  Incres   76.48
% Utri     2966  lenU  1406460  Ltol  2.00E+00  Umax   2.0E+00  Ugrwth 2.0E+00
% Ltri      207  dense1      0  Lmax  2.00E+00  Akmax  0.0E+00  Agrwth 0.0E+00
% bump    59004  dense2      0  DUmax  2.0E+00  DUmin  2.3E-05  condU  8.5E+04

rank2 = options.Rank;
rows2 = p2(1:rank2);
S2    = S(rows2,:);  % These should be independent rows of S.

%-----------------------------------------------------------------------------
% Factor scaled SS with rook pivoting.
% It returns rank 61833 also.
disp(' ')
disp('Factor scaled S(transpose) now with rook pivoting')
tic
[L3,U3,p3,q3,options] = lusolFactor(SS',options);
toc

% m       75644 >n       62177  Elems   916437  Amax   1.0E+00  Density   0.02
% Singular(m>n)  rank    61833  n-rank     344  nsing      344
% Merit    12.7  lenL   571936  L+U    1015445  Cmpressns    0  Incres   10.80
% Utri      556  lenU   443509  Ltol  2.00E+00  Umax   3.2E+01  Ugrwth 3.2E+01
% Ltri     2959  dense1      0  Lmax  2.00E+00
% bump    72129  dense2      0  DUmax  2.0E+01  DUmin  5.4E-05  condU  3.8E+05

rank3 = options.Rank;
rows3 = q3(1:rank3);
S3    = S(rows3,:);  % These should be independent rows of S.

disp(' ')
disp('rows1, rows2, rows3 are 3 sets of independent rows of S')
disp('   S1,    S2,    S3 are those submatrices of S')
disp(' ')


function [vertex,faces] = compute_saddle_points(Q,D,mask)

% compute_saddle_points - compute saddle points of a Voronoi segmentation
%
%   [vertex,faces] = compute_saddle_points(Q,D[,mask]);
%
%   Q is a voronoi index map.
%   D is the distance function associated to the voronoi map.
%
%   The vertex of the saddle points are first the double points (meeting
%   points of the voronoi diagram along the boundary of the domain) and
%   then the triple points (meeting points of 3 cells).
%
%   Copyright (c) 2008 Gabriel Peyre

if nargin==3 && not(isempty(mask))
    Q(mask==0) = -1;
end

Q1 = zeros(size(Q)+2)-1;
Q1(2:end-1,2:end-1) = Q;
V = [];
v = Q1(1:end-1,1:end-1); V = [V v(:)];
v = Q1(2:end,1:end-1); V = [V v(:)];
v = Q1(1:end-1,2:end); V = [V v(:)];
v = Q1(2:end,2:end); V = [V v(:)];
V = sort(V,2);
d = (V(:,1)~=V(:,2)) + (V(:,2)~=V(:,3)) + (V(:,3)~=V(:,4));
V = V';

I = find(d>=2);

[vx,vy] = ind2sub(size(Q)+1, I);
vx = clamp(vx,1,size(Q,1));
vy = clamp(vy,1,size(Q,1));
J = vx+(vy-1)*size(Q,1);

% sort according to distance
[tmp,s] = sort(D(J), 1, 'descend');
I = I(s);

[vx,vy] = ind2sub(size(Q)+1, I);
vx = clamp(vx,1,size(Q,1));
vy = clamp(vy,1,size(Q,1));
vertex = cat(1,vx',vy');

V = sort(V, 1, 'descend');
faces = V(1:3, I);

if isempty(vertex)
    % add farthest point
    [tmp,I] = max( D(:) );
    [vx,vy] = ind2sub(size(D), I(1));
    vertex = [vx;vy];
    faces = [-1 -1 -1]';
end

I = find( faces(1,:)<0 | faces(2,:)<0 | faces(3,:)<0 );
J = find( faces(1,:)>0 & faces(2,:)>0 & faces(3,:)>0 );
faces = cat(2, faces(:,I), faces(:,J) );


function [ algam, sgngam ] = r8_lgams ( x )

%*****************************************************************************80
%
%% R8_LGAMS evaluates the log of |gamma(x)| and sign, for an R8 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real ALGAM, the logarithm of the absolute value of
%    gamma ( X ).
%
%    Output, real SGNGAM, the sign (+1 or -1 ) of gamma ( X ).
%
  algam = r8_lngam ( x );
  sgngam = 1.0;

  if ( x <= 0.0 )

    k = floor ( mod ( - r8_aint ( x ), 2.0 ) + 0.1 );

    if ( k == 0 )
      sgngam = - 1.0;
    end

  end

  return
end