function [x,info,Ai,Bi,BTi,Res,Pro,isFreeDof] = mg(A,b,elem,option,varargin)
%% MG multigrid-type solvers
%
% x = MG(A,b,elem) attempts to solve the system of linear equations A*x =
% b for x using geometric multigrid solvers. Inside mg, an coarsening algorithm
% is applied. See coarsen for the coarsening algorithm on bisection grids.
%
% The method is designed for the system from several finite element
% descritzations of elliptic equations on a grid whose topology
% is given by the array elem.
%
% - 2D: P0, P1, P2, P3, CR, WG
% - 3D: P0, P1, P2, CR, WG
%
% The algorithm is based on fast auxiliary space preconditioner (FASP).
% We first transfer a given element to P1 element and then build V-cycle
% for P1 element. The whole cycle is used as a preconditioner in CG to
% solve the linear system.
%
% Reference: J. Xu, The auxiliary space method and optimal multigrid
% preconditioning techniques for unstructured grids, Computing. 56 (1996)
% 215?235.
%
% The type of the finite element can be build into the size of the problem.
%
% - NT: number of elements         P0 element
% - N : number of vertices         P1 element
% - NE: number of edges            CR element in 2D
% - NF: number of faces            CR element in 3D
% - N + NE: sum of vertices and edges    P2 element
% - N + 2*NE + NT:                       P3 element in 2D
%
% For Dirichlet problems, usually the matrix equation can be restricted to
% free dofs and results a smaller matrix in which the link of the size of
% the matrix and the type of elements is missing. In this case, use
% option.freeDof to provide a logic array for free dofs and the length of
% option.freeDof can be used to determine the type.
%
% So for Dirichlet problems, use
% - mg(A,b,elem,option)      with option.freeDof is given
% - mg(AD,b,elem)       where AD is a larger matrix containing boundary dof
%
% For elements involving dof on edges and faces, additional mesh structure
% such as |edge|, |face| can be provided. Otherwise, the subroutine will
% generate them using |elem|.
%
% For 3-D *adaptive meshes*, HB information is needed, and should be
% listed as the first parameter in varargin.
%
%   - mg(A,b,elem,option,HB)            3-D linear P1 element
%   - mg(A,b,elem,option,HB,edge)       3-D quadratic P2 element
%   - mg(A,b,elem,option,HB,face)       3-D non-conforming CR P1 element
%
% For 3-D meshes obtained by uniformrefine3 or uniformbisect3, no HB is
% needed and simple mg(A,b,elem) is okay.
%
% In some applications, multigrid solvers are used as a build-in block of
% other larger systems. Then use
%
% [~,~,Ai,Bi,BBi,Res,Pro,isFreeDof] = mg(A,f,elem,setupOption);
%
% with setupOption.solver = 'NO' to get hierarichal structure and then use
%
%   e = mg(A,r,elem,option,Ai,Bi,BBi,Res,Pro,isFreeDof);
%
% This will save the setting time. See diapreStokes, tripremixPoisson.
%
% When testing the solvers, more options can be provided.
%
%   x = MG(A,b,elem,options) specifies options in the following list.
%   - option.x0: the initial guess. Default setting x0 = 0.
%   - option.tol: the tolerance of the convergence. Default setting 1e-8.
%   - option.maxIt: the maximum number of iterations. Default setting 200.
%   - option.N0: the size of the coarest grid. Default setting 500.
%   - option.mu: smoothing steps. Default setting 1
%   - option.coarsegridsolver: solver used in the coarest grid. Default
%     setting: direct solver.
%   - option.freeDof: free d.o.f
%   - option.solver: various cycles and Krylov space methods
%       * 'NO'     only setup the transfer matrix
%       * 'Vcycle'      V-cycle MultiGrid Method
%       * 'Wcycle'      W-cycle MultiGrid Method
%       * 'Fcycle'      Full cycle Multigrid Method
%       * 'cg'     MG-Preconditioned Conjugate Gradient
%       * 'minres' MG-Preconditioned Minimal Residual Method
%       * 'gmres'  MG-Preconditioned Generalized Minimal Residual Method
%       * 'bicg'   MG-Preconditioned BiConjugate Gradient Method
%       * 'bicgstable' MG-Preconditioned BiConjugate Gradient Stabilized Method
%       * 'bicgstable1' MG-Preconditioned BiConjugate Gradient Stabilized Method
%       The default setting is 'cg' which works well for SPD matrices. For
%       non-symmetric matrices, try 'gmres' and for symmetric but indefinite
%       matrices, try 'minres' or 'bicg' sequences.
%       The string option.solver is not case sensitive.
%   - option.preconditioner:  multilevel preconditioners including:
%       * 'V'   V-cycle MultiGrid used as a Preconditioner
%       * 'W'   W-cycle MultiGrid used as a Preconditioner
%       * 'F'   Full cycle Multigrid used as a Preconditioner
%       * 'bpx' BPX-Preconditioner
%   - option.printlevel: the level of screen print out
%       * 0: no output
%       * 1: name of solver and convergence information (step, err, time)
%       * 2: convergence history (err in each iteration step)
%
%   [x,info] = MG(A,b,elem) also returns information of the solver
%   - info.flag:
%       * 0: mg converged to the desired tolerance tol within maxIt iterations
%       * 1: mg iterated maxIt times but did not converge.
%       * 2: direct solver
%   - info.itStep: the iteration number at which x was computed.
%   - info.time: the cpu time to get x
%   - info.err: the approximate relative error in the energy norm in
%   err(:,1) and the relative residual norm(b-A*x)/norm(b) in err(:,2). If
%   flag is 0, then max(err(end,:)) <= tol.
%   - info.stopErr: the error when iteration stops
%
%   Example:
%
%   [node,elem] = squaremesh([0 1 0 1],0.5);
%   for k = 1:8
%     [node,elem] = uniformrefine(node,elem);
%   end
%   pde.f = inline('p(:,1).*p(:,2)','p');
%   pde.g_D = inline('zeros(size(p,1),1)','p');
%   option.solver = 'notsolve';
%   [soln,eqn] = Poisson(node,elem,[],pde,option);
%   fprintf('\n Number of unknowns: %8.0u\n',length(eqn.b))
%   tic; display('Direct solver'); u = eqn.A\eqn.b; toc;
%   tic; x = mg(eqn.A,eqn.b,elem); toc;
%   format shorte
%   fprintf('Difference between direct and mg solvers %0.2g \n',norm(u-x));
%
% See also mgMaxwell, mgstokes
%
% Documentation in Help browser ifem mgdoc
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

t = cputime;
%% Size of systems
Nborig = size(b,1);                % number of dof
Ndof = Nborig;
nb = size(b,2);                    % number of bs
N = max(elem(:));                  % number of nodes
NT = size(elem,1);                 % number of elements
dim = size(elem,2)-1;
if N > NT       % 2-D quad mesh
    dim = 2;
end

%% Options
% Assign default values to unspecified parameters
if ~exist('option','var')
    option = [];
end
option = mgoptions(option,Nborig);    % parameters
x0 = option.x0;
if size(x0,2) == 1
    x0 = repmat(x0,1,nb);
end
N0 = option.N0;
tol = option.tol;
maxIt = option.solvermaxit;
mu = option.smoothingstep;
smoothingRatio = option.smoothingratio; % for variable smoothing
solver = option.solver;
preconditioner = option.preconditioner;
coarsegridsolver = option.coarsegridsolver;
printlevel = option.printlevel;
setupflag = option.setupflag;
if nargin > 8      % with the multilevel structure in the input
    setupflag = 0;
end

%% Set up multilevel structure
if setupflag == true
%% eliminate isolated dof
if isfield(option,'freeDof') % freeDof is given
   if islogical(option.freeDof)
      Ndof = max(length(option.freeDof),Nborig);
   else
      error('Provide logic array for option.freeDof');
   end
   isFreeDof = false(Ndof,1);
   isFreeDof(option.freeDof) = true;
   isFixDof = true(Ndof,1);
   isFixDof(isFreeDof) = false;
else % find free dofs and eliminate isolated dofs
    Ndof = Nborig;
    degreeA = sum(spones(A));  % degree
    isFreeDof = false(size(A,1),1);
    isFreeDof(degreeA>1) = true;
    isFixDof = ~isFreeDof;
    isFixDof(degreeA == 0) = false;
end
if Nborig > sum(isFreeDof) % truncate the matrix
    xD = zeros(Nborig,nb);
    Nfix = sum(isFixDof);
    ADinv = spdiags(1./diag(A(isFixDof,isFixDof)),0,Nfix,Nfix);
    xD(isFixDof,:) = ADinv*b(isFixDof,:);
    A = A(isFreeDof,isFreeDof);
end
if Nborig > sum(isFreeDof) % truncate the rhs
    b = b(isFreeDof,:);
end
if size(x0,1) > sum(isFreeDof) % truncate the initial guess
    x0 = x0(isFreeDof,:);
    option.x0 = x0;
end
isFreeNode = isFreeDof(1:N); % the first N element is for P1

%% Additional mesh structure for different elements
% list of elements: P0,P2,P3,CR,HB,WG
if Ndof > N % other than P1 element
    NE = N + NT -1; % estimate of NE by Euler formula
    if dim == 3
        NF = 2*NT;  % estimate of NF
    end
    % get additional data structure
    if Ndof ~= NT   % not piecewise constant element
        if dim == 2
            if nargin > 4
                edge = varargin{1};
                NE = size(edge,1);
            else
                [tempvar,edge] = dofedge(elem);
            end
        elseif dim == 3
            if nargin > 5 % additional data structure is in the input
                if size(varargin{1},2) == 2 % edge
                    edge = varargin{1};
                    NE = size(edge,1);
                elseif size(varargin{1},2) == 3 % face
                    face = varargin{1};
                    NF = size(face,1);
                end
            else % if no additional edge/face is input, generate it
                 [tempvar,edge] = dof3edge(elem); %#ok<*ASGLU>
                 [tempvar,face] = dof3face(elem);
                 NE = size(edge,1);
                 NF = size(face,1);
            end
        end
    end
    % transfer operators from P1 to the current element
    isFreeNode = true(N,1);
    if Ndof == NT % piecewise constant element
       if dim == 2
          P1toP0 = sparse([1:NT;1:NT;1:NT]',elem,ones(3*NT,1)/3,NT,N);
       elseif dim == 3
          P1toP0 = sparse([1:NT;1:NT;1:NT;1:NT]',elem,ones(4*NT,1)/4,NT,N);
       end
       auxPro = P1toP0(isFreeDof,1:N);
       isFreeNode = [];
    end
    if Ndof == N + NE % quadratic element
        isFreeNode = isFreeDof(1:N);
        P1toP2 = sparse([(1:N)'; N+(1:NE)'; N+(1:NE)'], ...
                        [(1:N)'; double(edge(:))],...
                        [ones(N,1); 0.5*ones(2*NE,1)]',Ndof,N);
        auxPro = P1toP2(isFreeDof,isFreeNode);
    end
    if (dim == 2) && (Ndof == N + 2*NE +NT) % cubic element in 2-D
        isFreeNode = isFreeDof(1:N);
        P1toP3 = sparse([(1:N)'; N+2*(1:NE)'-1; N+2*(1:NE)'-1; N+2*(1:NE)';...
                          N+2*(1:NE)';N+2*NE+(1:NT)';N+2*NE+(1:NT)';N+2*NE+(1:NT)'], ...
                        [(1:N)'; double(edge(:));double(edge(:));elem(:);],...
                        [ones(N,1); 2/3*ones(NE,1);1/3*ones(NE,1);...
                         1/3*ones(NE,1);2/3*ones(NE,1);1/3*ones(3*NT,1)]',Ndof,N);
         auxPro = P1toP3(isFreeDof,isFreeNode);
    end
    if (dim == 2) && (Ndof == NE)  % 2-D CR nonconforming element
        isFreeNode(edge(isFixDof,:)) = false;
        P1toCR = sparse([1:NE;1:NE]',double(edge(:)),0.5*ones(2*NE,1),NE,N);
        auxPro = P1toCR(isFreeDof,isFreeNode);
    end
    if (dim == 3) && (Ndof == NF) % 3-D CR nonconforming element
        isFreeNode(face(isFixDof,:)) = false;
        P1toCR = sparse([1:NF;1:NF;1:NF]',double(face(:)),ones(3*NF,1)/3,NF,N);
        auxPro = P1toCR(isFreeDof,isFreeNode);
    end
    if (dim == 2) && (Ndof == NT + NE) % 2-D weak Galerkin element (P0,P0,RT0) element
        fixEdgeDof = find(isFixDof) - NT;
        isFreeNode(edge(fixEdgeDof,:)) = false;
        P1toWG = sparse([repmat((1:NT)',3,1); repmat((NT+1:Ndof)',2,1)], ... % i
                        [elem(:);             double(edge(:))], ...   % j
                        [ones(3*NT,1)/3;      ones(2*NE,1)/2], NT+NE, N);
        auxPro = P1toWG(isFreeDof,isFreeNode);
    end
    if (dim == 3) && (Ndof == NT + NF) % 3-D weak Galerkin element (P0,P0,RT0) element
        fixFaceDof = find(isFixDof) - NT;
        isFreeNode(face(fixFaceDof,:)) = false;
        P1toWG = sparse([repmat((1:NT)',4,1); repmat((NT+1:Ndof)',3,1)], ... % i
                        [elem(:);           double(face(:))], ...   % j
                        [ones(4*NT,1)/4;    ones(3*NF,1)/3], NT+NF, N);
        auxPro = P1toWG(isFreeDof,isFreeNode);
    end
    if (nargin > 4) && (dim == 2) && ischar(varargin{1}) && strcmp(varargin{1},'HB') % HB basis element
        isFreeNode = isFreeDof(1:N);
        P1toPk = speye(Ndof,N);
        auxPro = P1toPk(isFreeDof,isFreeNode);
    end
end

%% Hierarchical Structure of Mesh
if dim == 2  % 2-D
    [HB, NL, level] = HBstructure(elem,N0);
end
if dim == 3  % 3-D
    if nargin > 4
        HBmesh = varargin{end}; % need HB for bisection refinement
        [HB, NL, level] = HBstructure3(elem,N0,HBmesh);
    else % no HBmesh is given. only work for the red uniform refinement
        [HB, NL, level] = HBstructure3(elem,N0);
    end
end

%% Transfer operators between multilevel meshes for P1 element
% standard prolongation and restriction operator for P1 element
[Pro,Res] = transferoperator(HB,NL,isFreeNode);
% add one more level from P1 to the current element
if Ndof > N
    if ~exist('auxPro','var')
        disp('The current element is not supported by mg');
    else
        Pro{level} = auxPro;
        Res{level+1} = Pro{level}';
        level = level + 1;
    end
end
clear HB auxPro

%% Matrices in each level
Ai = cell(level,1);
Ai{level} = A;
if level == 1
    Bi{1} = tril(Ai{1});
    BTi{1} = transpose(Bi{1});
    Res = []; Pro = [];
end
for j = level:-1:2
    Ai{j-1} = Res{j}*Ai{j}*Pro{j-1};           % Ac = Res*Af*Pro
    switch option.smoother
        case 'GS'
            Bi{j} = tril(Ai{j});        % Forward Gauss-Seidel   B = D+L
            BTi{j} = triu(Ai{j});       % Backward Gauss-Seidel BT = D+U
        case 'JAC'                      % Jacobi iteration  B = BT = D;
            Bi{j} = spdiags(diag(Ai{j}),0,size(Ai{j},1),size(Ai{j},1));
            BTi{j} = Bi{j};
    end
    if option.smoothingparameter~=1     % scaling of the smoother
        Bi{j} = Bi{j}/option.smoothingparameter;
        BTi{j} = BTi{j}/option.smoothingparameter;
    end
end
end % end for setup

%% Only set up the transfer operators
if strcmp(solver,'NO')
    x = x0; flag = 0; itStep = 0; err = 0; time = cputime-t;
    info = struct('solverTime',time,'itStep',itStep,'error',err,'flag',flag,'stopErr',max(err(end,:)));
    return
end

%% No need of set up
if setupflag == false
    Ai = varargin{1};
    Bi = varargin{2};
   BTi = varargin{3};
   Res = varargin{4};
   Pro = varargin{5};
   if length(varargin)>=6
      isFreeDof = varargin{6};
   else
      isFreeDof = true(Ndof,1);
   end
   level = length(Ai);
   isFixDof = ~isFreeDof;
   xD = zeros(Ndof,nb);
   if any(isFixDof) && Ndof == length(isFreeDof) % larger system
        Nfix = sum(isFixDof);
        ADinv = spdiags(1./diag(A(isFixDof,isFixDof)),0,Nfix,Nfix);
        xD(isFixDof,:) = ADinv*b(isFixDof,:);
        b = b(isFreeDof,:);
        A = A(isFreeDof,isFreeDof);
        x0 = x0(isFreeDof,:);
   end
end
if condest(Ai{1}) > 1e16 % Ai{1} is singular
    Ai{1} = Ai{1} + 1e-12*speye(size(Ai{1}));
%     coarsegridsolver = 'pcg';
end

%% No coarsening or coarsened nodes is small
if Ndof <= N    % linear element
    if level == 1
        if Ndof < N0 % small size
            solver = 'DIRECT';
        else
            [x,info] = amg(A,b,option);
            if any(isFixDof) && Nborig > size(x,1) % a larger system
                xD(isFreeDof,:) = x;
                x = xD;
            end
            Ai{1} = A; Bi{1} = tril(A); BTi{1} = Bi'; Res = []; Pro = [];
            fprintf('No hierarchical structure of the mesh is found. AMG is used. \n');
            return
        end
    end
end

%% Direct solver
if strcmp(solver,'DIRECT')
    x = A\b;                       % use direct solver
    flag = 2; itStep = 0; err = norm(b-A*x)/norm(b); time = cputime - t;
    %% Modify x to include fix dof
    if any(isFixDof) && Nborig > size(x,1)
        xD(isFreeDof,:) = x;
        x = xD;
    end
    if printlevel >= 1
        fprintf('Direct solver \n')
        fprintf('#dof: %8.0u,  #nnz: %8.0u,  iter: %2.0u,  err = %8.4e,  time = %4.2g s\n',...
                 Ndof, nnz(A), itStep, max(err(end,:)), time)
        if level == 1
            fprintf('No coarsening of the grid. \n')
        end
    end
    info = struct('solverTime',time,'itStep',itStep,'error',err,'flag',flag,'stopErr',max(err(end,:)));
    Ai{1} = A; Bi{1} = tril(A); BTi{1} = Bi'; Res = []; Pro = [];
    return
end

%% Krylov iterative methods use Multigrid-type Preconditioners
% set up preconditioner
if ~strcmp(solver(2:end),'CYCLE')
    switch preconditioner
        case 'V'
            prefunc = @vcycle;
            if printlevel >= 1
                fprintf('\n Multigrid V-cycle Preconditioner with ')
            end
        case 'W'
            prefunc = @wcycle;
            if printlevel >= 1
                fprintf('\n Multigrid W-cycle Preconditioner with ')
            end
        case 'F'
            prefunc = @fcycle;
            if printlevel >= 1
                fprintf('\n Multigrid Full Cycle Preconditioner with ')
            end
        case 'BPX'
            % modify smoother
            for j = level:-1:2
                Di{j} = diag(Ai{j});
            end
            prefunc = @bpx;
            if printlevel >= 1
                fprintf('BPX Preconditioner with ')
            end
    end
end
% initial set up
k = 1;
x = x0;
r = b - A*x;
nb = max(sqrt(sum(b.^2,1)));
err = zeros(maxIt,2);
if nb > eps  % nb is non-zero
    err(1,:) = max(sqrt(sum(r.^2,1)))/nb;
else
    err(1,:) = max(sqrt(sum(r.^2,1)));
end
% solvers
switch solver
    case 'CG'
        if printlevel >= 1
            fprintf('Conjugate Gradient Method\n')
        end
        while (max(err(k,:)) > tol) && (k <= maxIt)
            % compute Br by MG
            Br = prefunc(r);
            % update tau, beta, and p
            rho = dot(Br,r);  % e'*ABA*e approximates e'*A*e
            if k == 1
                p = Br;
            else
                beta = rho./rho_old;
                p = Br + beta.*p;
            end
            % update alpha, x, and r
            Ap = A*p;
            alpha = rho./dot(Ap,p);
            r = r - alpha.*Ap;
            x = x + alpha.*p;
            rho_old = rho;
            k = k + 1;
            % compute err for the stopping criterion
        %     err(k,1) = alpha*sqrt(p'*Ap/(x'*A*x)); % increamental error in energy norm
            err(k,1) = max(sqrt(abs(rho./dot(x,b)))); % approximate relative error in energy norm
            if nb > eps
                err(k,2) = max(sqrt(sum(r.^2,1)))/nb; % relative error of the residual in L2-norm
            else
                err(k,2) = max(sqrt(sum(r.^2,1)));
            end
            if printlevel >= 2
                fprintf('#dof: %8.0u,  #nnz: %8.0u, MGCG iter: %2.0u, err = %8.4e\n',...
                         Ndof, nnz(A), k-1, max(err(k,:)));
            end
        end
        err = err(1:k,:);
        itStep = k-1;
    case 'VCYCLE'
        if printlevel >= 1
            fprintf('\n Multigrid Vcycle Iteration \n')
        end
        while (max(err(k,:)) > tol) && (k <= maxIt)
            k = k + 1;
            % Step 2: Compute Br by one Vcylce MG
            Br = vcycle(r);
            % Step 3: Correct the solution
            x = x + Br;
            % Step 1: Form residual r
            r = r - A*Br;
            err(k,1) = max(sqrt(abs(dot(Br,r)/dot(x,b)))); % approximate relative error in energy norm
            if nb > eps
                err(k,2) = max(sqrt(sum(r.^2,1)))/nb; % relative error of the residual in L2-norm
            else
                err(k,2) = max(sqrt(sum(r.^2,1)));
            end
            if printlevel >= 2
                fprintf('#dof: %8.0u,  #nnz: %8.0u, MG Vcycle iter: %2.0u, err = %8.4e\n',...
                         Ndof, nnz(A), k-1, max(err(k,:)));
            end
        end
        err = err(1:k,:);
        itStep = k-1;
    case 'WCYCLE'
        if printlevel >= 1
            fprintf('\n Multigrid Wcycle Iteration \n')
        end
        while (max(err(k,:)) > tol) && (k <= maxIt)
            k = k + 1;
            % Step 2: Compute Br by one Vcylce MG
            Br = wcycle(r);
            % Step 3: Correct the solution
            x = x + Br;
            err(k,1) = max(sqrt(abs(dot(Br,r)/dot(x,b)))); % approximate relative error in energy norm
            % Step 1: Form residual r
            r = r - A*Br;
            if nb > eps
                err(k,2) = max(sqrt(sum(r.^2,1)))/nb; % relative error of the residual in L2-norm
            else
                err(k,2) = max(sqrt(sum(r.^2,1)));
            end
            if printlevel >= 2
                fprintf('#dof: %8.0u,  #nnz: %8.0u, MG Wcycle iter: %2.0u, err = %8.4e\n',...
                         Ndof, nnz(A), k-1, max(err(k,:)));
            end
        end
        err = err(1:k,:);
        itStep = k-1;
    case 'FCYCLE'
        if printlevel >= 1
            fprintf('\n Multigrid Full Cycle Iteration \n')
        end
        while (max(err(k,:)) > tol) && (k <= maxIt)
            k = k + 1;
            % Step 2: Compute Br by one Full cycle MG
            Br = fcycle(r);
            % Step 3: Correct the solution
            x = x + Br;
            err(k,1) = max(sqrt(abs(dot(Br,r)/dot(x,b)))); % approximate relative error in energy norm
            % Step 1: Form residual r
            r = r - A*Br;
            if nb > eps
                err(k,2) = max(sqrt(sum(r.^2,1)))/nb; % relative error of the residual in L2-norm
            else
                err(k,2) = max(sqrt(sum(r.^2,1)));
            end
            if printlevel >= 2
                fprintf('#dof: %8.0u, #nnz: %8.0u, MG Fcycle iter: %2.0u, err = %8.4e\n',...
                         Ndof, nnz(A), k-1, max(err(k,:)));
            end
        end
        err = err(1:k,:);
        itStep = k-1;
    case 'MINRES'
        if printlevel >= 1
            fprintf('Minimum Residual Method \n')
        end
        [x,flag,err,itStep] = minres(A,b,tol,maxIt,prefunc,[],x0);
    case 'GMRES'
        if printlevel >= 1
            fprintf('General Minimum Residual Method\n')
        end
        if isfield(option,'restart')
            restart = option.restart;
        else
            restart = min(N,10);
        end
        [x,flag,err,itStep] = gmres(A,b,restart,tol,maxIt,prefunc,[],x0);
        itStep = (itStep(1)-1)*restart + itStep(2);
    case 'BICG'
        if printlevel >= 1
            fprintf('BiConjugate Gradient Method\n')
        end
        [x,flag,err,itStep] = bicg(A,b,tol,maxIt,prefunc,[],x0);
    case 'BICGSTAB'
        if printlevel >= 1
            fprintf('Stablilized BiConjugate Gradient Method\n')
        end
        [x,flag,err,itStep] = bicgstab(A,b,tol,maxIt,prefunc,[],x0);
    case 'BICGSTAB1'
        if printlevel >= 1
            fprintf('Stablilized BiConjugate Gradient Method\n')
        end
        [x,flag,err,itStep] = bicgstab1(A,b,tol,maxIt,prefunc,[],x0);
end

%% Modify x to include fix dof
if Nborig > size(x,1) % a larger system
    xD(isFreeDof,:) = x;
    x = xD;
end

%% Output
if k > maxIt
    flag = 1;
else
    flag = 0;
end
time = cputime - t;
if printlevel >= 2
    fprintf('#dof: %8.0u, level: %2.0u,   coarse grid %2.0u, #nnz: %8.0u\n',...
              size(b,1), level, size(Ai{1},1), nnz(Ai{1}))
end
if printlevel >= 1
    fprintf('#dof: %8.0u,  #nnz: %8.0u, smoothing: (%1.0u,%1.0u), iter: %2.0u,   err = %8.2e,   time = %4.2g s\n',...
                 size(b,1), nnz(A), mu, mu, itStep, max(err(end,:)), time)
end
if (flag == 1) && (printlevel>0)
   fprintf('NOTE: the iterative method does not converge! \n');
end
info = struct('solverTime',time,'itStep',itStep,'error',err,'flag',flag,'stopErr',max(err(end,:)));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions vcycle, wcycle, fcycle, bpx
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Vcycle MG
    function Br = vcycle(r,J)  % solve equations Ae = r in each level
    if nargin<=1
        J = level;
    end
    ri = cell(J,1);            % residual in each level
    ei = cell(J,1);            % correction in each level
    mui = cell(J,1);           % variable smoothing steps
    ri{J} = r;
    mui{J} = mu;
    for i = J:-1:2
        ei{i} = Bi{i}\ri{i};   % pre-smoothing
        for s = 1:mui{i}-1     % extra mu-1 steps smoothing
            if mod(s,2)        % switch between B and BT to symmetrize smoother
                ei{i} = ei{i} + BTi{i}\(ri{i}-Ai{i}*ei{i});
            else
                ei{i} = ei{i} + Bi{i}\(ri{i}-Ai{i}*ei{i});
            end
        end
        ri{i-1} = Res{i}*(ri{i} - Ai{i}*ei{i});
        mui{i-1} = ceil(mui{i}*smoothingRatio);
    end
    switch coarsegridsolver
        case 'direct'
            ei{1} = Ai{1}\ri{1};   % direct solver in the coarest level
        case 'pcg'
            D = spdiags(diag(Ai{1}),0,size(Ai{1},1),size(Ai{1},1));
            [ei{1},flag] = pcg(Ai{1},ri{1},1/size(Ai{1},1),1000,D);
        case 'amg'
            amgoption.printlevel = 0;
            ei{1} = amg(Ai{1},ri{1},amgoption);
    end
    for i = 2:J
        ei{i} = ei{i} + Pro{i-1}*ei{i-1};
        for s = 1:mui{i}       % post-smoothing
            if mod(s,2)        % switch between B and BT to symmetrize smoother
                ei{i} = ei{i} + BTi{i}\(ri{i}-Ai{i}*ei{i});
            else
                ei{i} = ei{i} + Bi{i}\(ri{i}-Ai{i}*ei{i});
            end
        end
    end
    Br = ei{J};
    end

%% Wcycle MG
    function e = wcycle(r,J)    % solve equations Ae = r in level J
    if nargin<=1
        J = level;
    end
    if J == 1
        switch coarsegridsolver
            case 'direct'
                e = Ai{J}\r;   % direct solver in the coarest level
            case 'pcg'
                D = spdiags(diag(Ai{J}),0,size(Ai{J},1),size(Ai{J},1));
                [e,flag] = pcg(Ai{J},r,1/size(Ai{J},1),1000,D);
            case 'amg'
                amgoption.printlevel = 0;
                e = amg(Ai{J},r,amgoption.printlevel);
        end
        return
    end
    % fine grid pre-smoothing
    e = Bi{J}\r;        % pre-smoothing
    for s = 1:mu-1      % extra mu-1 steps smoothing
        if mod(s,2)
            e = e + Bi{J}\(r-Ai{J}*e);
        else
            e = e + BTi{J}\(r-Ai{J}*e);
        end
    end
    % restriction
    rc = Res{J}*(r - Ai{J}*e);
    % coarse grid correction twice
    ec = wcycle(rc,J-1);
    ec = ec + wcycle(rc - Ai{J-1}*ec,J-1);
    % prolongation
    e = e + Pro{J-1}*ec;
    % fine grid post-smoothing
    for s = 1:mu
        if mod(s,2)
            e = e + Bi{J}\(r-Ai{J}*e);
        else
            e = e + BTi{J}\(r-Ai{J}*e);
        end
    end
    end

%% Fcycle MG
    function Br = fcycle(r)
    ri = cell(level,1);            % residual in each level
    ei = cell(level,1);            % correction in each level
    ri{level} = r;
    for i = level:-1:2
        ei{i} = vcycle(ri{i},i);   % pre-smoothing
        for s = 1:mu-1             % extra smoothing steps
            ei{i} = ei{i} + vcycle(ri{i}-Ai{i}*ei{i},i); % pre-smoothing
        end
        ri{i-1} = Res{i}*(ri{i} - Ai{i}*ei{i});
    end
    switch coarsegridsolver
        case 'direct'
            ei{1} = Ai{1}\ri{1};   % direct solver in the coarest level
        case 'pcg'
            D = spdiags(diag(Ai{1}),0,size(Ai{1},1),size(Ai{1},1));
            [ei{1},flag] = pcg(Ai{1},ri{1},1/size(Ai{1},1),1000,D);
        case 'amg'
            amgoption.printlevel = 0;
            ei{1} = amg(Ai{1},ri{1},amgoption);
    end
    for i = 2:level
        ei{i} = ei{i} + Pro{i-1}*ei{i-1};
        for s = 1:mu   % post-smoothing
            ei{i} = ei{i} + vcycle(ri{i}-Ai{i}*ei{i},i); % post-smoothing
        end
    end
    Br = ei{level};
    end

%% BPX preconditioner
    function Br = bpx(r)
    ri = cell(level,1);            % record residual in each level
    ei = cell(level,1);            % record err in each level
    % compute Br by BPX
    ri{level} = r;
    for i = level:-1:2
        ei{i} = ri{i}./Di{i};       % Jacobi smoothing
        ri{i-1} = Res{i}*ri{i};     % restriction of the residual
    end
    switch coarsegridsolver
        case 'direct'
            ei{1} = Ai{1}\ri{1};   % direct solver in the coarest level
        case 'pcg'
            D = spdiags(diag(Ai{1}),0,size(Ai{1},1),size(Ai{1},1));
            [ei{1},flag] = pcg(Ai{1},ri{1},1/size(Ai{1},1),1000,D);
        case 'amg'
            amgoption.printlevel = 0;
            ei{1} = amg(Ai{1},ri{1},amgoption);
    end
    for i=2:level
        ei{i} = ei{i} + Pro{i-1}*ei{i-1};  % prolongation of the correction
    end
    Br = ei{level};
    end
end %% end of mg


function[varargout]=ridgeinterp(varargin)
% RIDGEINTERP  Interpolate quantity values onto ridge locations.
%
%   RIDGEINTERP is a low-level function called by RIDGEWALK.
%
%   XI=RIDGEINTERP(FS,RQ,IR,JR,X) where RQ is a "ridge quantity" at
%   *radian* frequencies FS, and IR and JR are time and scale indices of
%   ridges, interpolates quantity X along the ridge locations to give XI.
%
%   IR and JR give indices into the first two dimensions of X.  The output
%   XI has the the same number of rows and columns as IR and JR, that is,
%   SIZE(XI,1)=SIZE(IR,1) and SIZE(XI,2)=SIZE(IR,2).  The locations of
%   NANs in IR and JR are duplicated in XI.
%
%   X may have more than one 'page' along its third dimension, in which
%   case each page is interpolated separately, and SIZE(XI,3)=SIZE(X,3).
%
%   RQ is output by ISRIDGEPOINT based on a wavelet transform output by
%   WAVETRANS, and IR and JR are output by RIDGEWALK.
%
%   RIDGEINTERP interpolates transform values between discrete frequency
%   levels to find a more precise value of the transform along the ridges
%   than simply looking up the values of X at rows IR and columns JR.
%
%   XI=RIDGEINTERP(FS,RQ,IR,JR,MU,X) interprets the ridges as belonging
%   to the MU-th derivative of the signal to which the quantity X belongs
%   and applies an appropriate correction factor.
%
%   [XI1,XI2,...,XIN]=RIDGEINTERP(FS,RQ,IR,JR,X1,X2,...,XN) also
%   interpolates the quantities X1,X2,...,XN, all having the same number of
%   rows and columns as IR and JR.
%
%   RIDGEINTERP uses fast quadratic interpolation via QUADINTERP.  In rare
%   cases, quadratic interpolation fails for an individual point and
%   therefore linear interpolation is used instead.
%   __________________________________________________________________
%
%   RIDGEINTERP is a low-level function called by RIDGEWALK.
%
%   See also RIDGEWALK, ISRIDGEPOINT, QUADINTERP.
%
%   Usage:  xi=ridgeinterp(rq,ir,jr,x);
%           [xi1,xi2,xi3]=ridgeinterp(rq,ir,jr,x1,x2,x3);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2005--2019 J.M. Lilly --- type 'help jlab_license' for details

rq=varargin{1};
ir=varargin{2};
jr=varargin{3};
varargin=varargin(4:end);

if iscell(varargin{end})||isempty(varargin{end})
    derivparams=varargin{end};
    varargin=varargin(1:end-1);
else
    derivparams=[];
end

index=find(~isnan(ir));
if ~isempty(index)
    varargout=ridgeinterp1_quadratic(index,ir,jr,derivparams,rq,varargin);
else
    for i=1:length(varargin)
        varargout{i}=[];
    end
end

function[outargs]=ridgeinterp1_quadratic(index,ir,jr,derivparams,rq,args)
sizeir=size(ir);
vcolon(ir,jr);
vindex(ir,jr,index,1);

indexr=nonnan(sub2ind(size(rq),ir,jr));

%Ridge quantity along the ridges, and at one scale up and down

di=size(rq,1);
dr=rq(indexr);
drp=rq(indexr+di);
drn=rq(indexr-di);

[~,jre]=quadinterp(jr-1,jr,jr+1,abs(drn).^2,abs(dr).^2,abs(drp).^2);

%Rare complete failure of quadratic interpolation associated
%with the ridge quantity changing sign between jrp and jrn, yet
%not having the ridge quantity at jr being between these two values

bool=~( (jr+1>jre) & (jre> jr-1));
jre(bool)=lininterp(jr(bool)-1,jr(bool)+1,drn(bool),drp(bool));

if ~isempty(derivparams)
    ga=derivparams{1};
    be=derivparams{2};
    mu=derivparams{3};

    fact=frac(morsefreq(ga,be-mu),morsefreq(ga,be));

    jro=frac(jr,fact);
    jrp=frac(jr+1,fact);
    jrn=frac(jr-1,fact);

    btop=(ceil(jro)>=(size(rq,2)-1));
    jro(btop)=size(rq,2)-1;
    jrp(btop)=size(rq,2);
    jrn(btop)=size(rq,2)-2;

    bbottom=(floor(jro)<=2);
    jro(bbottom)=2;
    jrp(bbottom)=3;
    jrn(bbottom)=1;

    %The problem here is that I also have to interpolate to find the
    %values of the quantities to be interpolated at the rescaled scale
    %levels, so you have to do another linear interpolation between
    %the floors and the ceilings.  You can't just do the above any
    %more because the rescaled scale levels are no longer jr+/-1.

%     figure,plot(jro)
%     maxmax(jro)
%     minmin(jro)
    indexrf=nonnan(sub2ind(size(rq),ir,floor(jro)));
    indexrc=nonnan(sub2ind(size(rq),ir,ceil(jro)));

    indexrpf=nonnan(sub2ind(size(rq),ir,floor(jrp)));
    indexrpc=nonnan(sub2ind(size(rq),ir,ceil(jrp)));

    indexrnf=nonnan(sub2ind(size(rq),ir,floor(jrn)));
    indexrnc=nonnan(sub2ind(size(rq),ir,ceil(jrn)));
end

for i=1:length(args)
   x=args{i};
   if isreal(x)
        xr=nan*ones(sizeir(1),size(x,3));
   else
        xr=(nan+sqrt(-1)*nan)*ones(sizeir(1),size(x,3));
   end
   for k=1:size(x,3)
        xk=x(:,:,k);
        if isempty(derivparams)
            xro=xk(indexr);
            xrp=xk(indexr+di);
            xrn=xk(indexr-di);
        else
            xro=lininterp(floor(jro),ceil(jro),xk(indexrf),xk(indexrc),jro);
            xrp=lininterp(floor(jrp),ceil(jrp),xk(indexrpf),xk(indexrpc),jrp);
            xrn=lininterp(floor(jrn),ceil(jrn),xk(indexrnf),xk(indexrnc),jrn);
        end

        xrk=quadinterp(jr-1,jr,jr+1,xrn,xro,xrp,jre);
        %Use linear interpolation where quadratic fails
        xrk(bool)=lininterp(jr(bool)-1,jr(bool)+1,xrn(bool),xrp(bool),jre(bool));
        xr(index,k)=xrk;
   end
   %Linearly interpolate between the approximate ridge and the bracketing curve
   outargs{i}=xr;
end


function clebsch_gordan_values_test ( )

%*****************************************************************************80
%
%% CLEBSCH_GORDAN_VALUES_TEST demonstrates the use of CLEBSCH_GORDAN_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 February 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CLEBSCH_GORDAN_VALUES_TEST:\n' );
  fprintf ( 1, '  CLEBSCH_GORDAN_VALUES returns values of \n' );
  fprintf ( 1, '  the Clebsch-Gordan coefficient.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '      J1      J2      J3      M1      M2      M3        CG\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, j1, j2, j3, m1, m2, m3, fx ] = clebsch_gordan_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %6.2f  %6.2f  %6.2f  %6.2f  %6.2f  %6.2f  %24.16f\n', ...
      j1, j2, j3, m1, m2, m3, fx );

  end

  return
end


function boundary_display ( v, bi )

%% BOUNDARY_DISPLAY displays a boundary.
%
%  Discussion:
%
%    The boundary is assumed to consist of one or more closed curves.
%
%    Each curve is represented by a sequence of line segments, traced in
%    counterclockwise order. (Interior holes go in clockwise order.)
%
%  Parameters:
%
%    Input, real V(V_NUM,2), the coordinates of vertices used to define the curves.
%
%    Input, integer BI(BI_NUM), a sequence of indices into V.  Each closed curve
%    is defined by giving a sequence of indices, which is terminated by
%    repeating the starting index.  Thus, BI = { 3, 1, 5, 3, 4, 2, 9, 7, 4 }
%    describes two curves: ( 3, 1, 5, 3 ) and (4, 2, 9, 7, 4 ).
%
  bi_num = length ( bi );

  clf
  hold on;
  next = 1;
  s = bi(1);
  t2 = s;
  draw = 1;

  while ( next < bi_num )
    t1 = t2;
    next = next + 1;
    t2 = bi(next);
    if ( draw )
      line ( [ v(t1,1), v(t2,1) ], [ v(t1,2), v(t2,2) ], 'LineWidth', 2, 'Color', 'k' );
      if ( t2 == s )
        draw = 0;
      end
    else
      s = t2;
      draw = 1;
    end

  end

  plot ( v(:,1), v(:,2), 'r.', 'MarkerSize', 15 );

  axis equal
  grid on
  hold off

  return
end


% phd_test.m
% ====================================================>
% This is a test script which demonstrates the usage of the "SMC_PHD" class.
%
% SETUP:
%  * Before running the simulation, open "2_crossing_targets.mat" or "3_roaming_targets.mat" datasets, from the "datasets" folder
%  * The datasets have been extracted by simulating the motion of differential robots in a 2D-plane (x,y)
%  * The "gen_obs_cluttered_multi3" function takes as an input the ground truth data, including information about the measurement noise and clutter rate
%     and then produces 1xNk cell array of corrupted and cluttered measurements, Nk being the total number of timesteps

% Load dataset
load('multiple-robot-tracking.mat');

tot_ellapsed = 0;
% Plot settings
ShowPlots = 1;              % Set to 0 to hide plots
ShowPrediction = 0;         % Set to 0 to skip showing prediction
ShowUpdate = 1;             % Set to 0 to skip showing update
SmoothTrajectories = 0;

% Recording settings
clear F;
Record = 1;                 % Set to (0|1) to turn video recording (off|on)
FrameRate = 10;            % Number of frames per second
VideoQuality = 100;         % Set to desired quality percentage
VideoPathName = 'tomb_tracks_only.avi'; % Set to the desired path and name of produced recording

lambdaV = 1; % Expected number of clutter measurements over entire surveillance region
V = 10^2;     % Volume of surveillance region (10x10 2D-grid)
V_bounds = [0 10 0 10]; % [x_min x_max y_min y_max]

% Instantiate a Transitionamic model
transition_model = ConstantVelocityX('NumDims',2,'VelocityErrVariance',0.0001);

% Instantiate a Measurement model
%measurement_model = LinearGaussianX('NumMeasDims',2,'NumStateDims',4,'MeasurementErrVariance',0.02,'Mapping',[1 3]);
measurement_model = RangeBearing2CartesianX('NumStateDims',4,'MeasurementErrVariance',[(pi/50)^2,0.02],'Mapping',[1 3]);

% Instantiate a clutter model
clutter_model = PoissonRateUniformPositionX('ClutterRate',lambdaV,'Limits',[V_bounds(1:2);V_bounds(3:4)]);

% Instantiate birth model
numBirthComponents = 10;
BirthComponents.Means = [ 1 9 9 1; 0 0 0 0; 1 1 9 9; 0 0 0 0];
BirthComponents.Covars = repmat(diag([2,0.1,2,0.1]),1,1,4);
BirthComponents.Weights = [.25, .25, .25, .25];
birth_distribution = GaussianMixtureX(BirthComponents.Means,BirthComponents.Covars, BirthComponents.Weights);
birth_model = DistributionBasedBirthModelX('Distribution', birth_distribution,...
                                           'BirthIntensity', 0.000001);

% Compile the State-Space model
ssm = StateSpaceModelX(transition_model,measurement_model,'Clutter',clutter_model, 'Birth', birth_model);

% Extract the ground truth data from the example workspace
load('example.mat');
NumIter = size(GroundTruth,2);

% Set BirthIntensity
NumTracks = 3;

% Generate DataList
meas_simulator = MultiTargetMeasurementSimulatorX('Model',ssm);
%meas_simulator.DetectionProbability = 1;
DataList = meas_simulator.simulate(GroundTruthStateSequence);

% Assign PHD parameter values
config.Model = ssm;
config.SurvivalProbability = 0.9;
config.DetectionProbability = 0.9;

% Instantiate PHD filter
filter = TrackOrientedMeMBerPoissonGMFilterX(config);
filter.Poisson.StatePosterior = copy(birth_distribution);

% Create figure windows
if(ShowPlots)
    img = imread('maze.png');

    % set the range of the axes
    % The image will be stretched to this.
    min_x = 0;
    max_x = 10;
    min_y = 0;
    max_y = 10;

    % make data to plot - just a line.
    x = min_x:max_x;
    y = (6/8)*x;

    figure('units','normalized','outerposition',[0 0 .5 1])
    ax(1) = gca;
    axis(ax(1), 'manual');
    figure('units','normalized','outerposition',[.5 0 .5 1])
    ax(2) = gca;

    axis
end

% START OF SIMULATION
% ===================>
for k=1:NumIter
    fprintf('Iteration = %d/%d\n================>\n',k,NumIter);

%     % Extract DataList at time k
%     tempDataList = DataList{k}(:,:);
%     tempDataList( :, ~any(tempDataList,1) ) = [];

    % Change PHD filter parameters
    filter.MeasurementList = DataList(k); % New observations

    tic;
    % Predict PHD filter
    filter.predict();

    % Update PHD filter
    filter.update();
    ellapsed = toc;
    tot_ellapsed = tot_ellapsed + ellapsed;
    %fprintf("Probability of existence: %f\n", filter.ProbOfExistence);
    fprintf("Ellapsed time: %f\n\n", ellapsed);
    % Plot update step results
    if(ShowPlots && ShowUpdate)
        % Plot data
        cla(ax(1));
         % Flip the image upside down before showing it

        % NOTE: if your image is RGB, you should use flipdim(img, 1) instead of flipud.
        hold on;
        axis(ax(1),V_bounds)
        hold(ax(1),'on');

        hold(ax(1),'on');
        for i=1:filter.Bernoulli.StatePosterior.NumComponents
            if(filter.Bernoulli.StatePosterior.Weights(i)>0.8)
                means = filter.Bernoulli.StatePosterior.Trajectories{i}.StateMean;
                if SmoothTrajectories
                    means = smoothdata(means','gaussian',10)';
                end
                plot(ax(1),means(1,:),means(3,:),'.-');
                plot_gaussian_ellipsoid(filter.Bernoulli.StatePosterior.Trajectories{i}.StateMean([1,3],end),filter.Bernoulli.StatePosterior.Trajectories{i}.StateCovar([1,3],[1,3],end),'r',1,50,ax(1));
            end
        end

        h2 = plot(ax(1), DataList(k).Vectors(1,:),DataList(k).Vectors(2,:),'k*','MarkerSize', 10);
        str = sprintf('Robot positions (Update)');
        title(ax(1),str)
        xlabel('X position (m)')
        ylabel('Y position (m)')

        % Plot PHD
        cla(ax(2), 'reset');
        p = filter.Bernoulli.StatePosterior.random(100000);
        [bandwidth,density,X,Y]=kde2d(p([1,3],:)');
        %contour3(X,Y,density,50);
        h = surf(ax(2),X,Y,density);
        shading interp
        colormap(ax(2), jet(3000))
        %set(h, 'edgecolor','none')
        hold on;
        plot(ax(2), filter.MeasurementList.Vectors(1,:), filter.MeasurementList.Vectors(2,:), 'y*');
        axis(ax(2), [V_bounds]);
        str = sprintf('PHD intensity (Update)');
        xlabel(ax(2),'X position (m)')
        ylabel(ax(2),'Y position (m)')
        zlabel(ax(2),'Intensity')
        title(ax(2),str)
        pause(0.01)

        % Store video frame
        if(Record)
            F(k) = getframe(ax(1));
        end
    end
end

% Create video file and write to it
if(Record)
    %F = F(2:end);
    vidObj = VideoWriter(char(VideoPathName));
    vidObj.Quality = VideoQuality;
    vidObj.FrameRate = FrameRate;
    open(vidObj);
    writeVideo(vidObj, F);
    close(vidObj);
end


function [l] = yd32l(yd3)
% Convert volume from cubic yards to liters.
% Chad Greene 2012
l = yd3*764.55485798;


function [BAS1,BAS2] = basIFE3DNed1coef(vert,intpt,coef)

%% USAGE: Coefficients of P1 IFE Shape Function on a Tetrahedron
%
% INPUTS:
% vert --- 4-by-3 matrix stores the
%          vertices of the tetrahedron
%          [A1;A2;A3;A4] = [x1 y1 z1; x2 y2 z2; x3 y3 z3; x4 y4 z4]
% intpt --- 3-by-3 or 4-by-3 matrix stores three intersection points
%          [D;E;F] = [xd yd zd; xe ye ze; xf yf zf];
%           the order is 1,2,3 or 2,3,4,5 of the index of edges
% coef --- [bt1,bt2] jump coefficient where bt1 is associated with A1.
%                                    bt2 is associated with A2 A3 A4
%
%     Type 1 Interface Element (3 intersection points), it means
%         A1 is on piece #1 whose coefficient is coef(1)
%         A2 A3 A4 are on piece #2 whose coefficient is coef(2)
%
%     Type 2 Interface Element (4 intersection points), it means
%         A1 A2 are on piece #1 whose coefficient is coef(1)
%         A3 A4 are on piece #2 whose coefficient is coef(2)
%
% OUTPUTS:
% BAS1 --- 6-by-6 matrix stores the coefficients of shape fun on piece #1
% BAS2 --- 6-by-6 matrix stores the coefficients of shape fun on piece #2
%          The ith basis: a * x + b on piece #j = 1 or 2
%              1st basis [a11,a12,a13,b11,b12,b13]
%              2nd basis [a21,a22,a23,b21,b22,b23]
%              3rd basis [a31,a32,a33,b31,b32,b33]
%              4th basis [a41,a42,a43,b41,b42,b43]
%              5th basis [a51,a52,a53,b51,b52,b53]
%              6th basis [a61,a62,a63,b61,b62,b63]
%
% Last Modified by Ruchi Guo on 19/11/20

%% 0. Initialization
bt1 = coef(1);  bt2 = coef(2); rb = bt2/bt1;
at1 = coef(3);  at2 = coef(4); ra = at2/at1;
 M = zeros(6,6); l = zeros(6,2); % the 2nd component is only associated with the shape fun on the piece2
if size(intpt,1) == 3
    D = intpt(1,:); E = intpt(2,:); F = intpt(3,:); C = 1/3*(D+E+F);
    M(1,1:3) = 1/2*(vert(1,:)+D); l(1,1) = norm(D-vert(1,:));
    M(1,4:6) = 1/2*(vert(2,:)+D); l(1,2) = norm(D-vert(2,:));
    M(2,1:3) = 1/2*(vert(1,:)+E); l(2,1) = norm(E-vert(1,:));
    M(2,4:6) = 1/2*(vert(3,:)+E); l(2,2) = norm(E-vert(3,:));
    M(3,1:3) = 1/2*(vert(1,:)+F); l(3,1) = norm(F-vert(1,:));
    M(3,4:6) = 1/2*(vert(4,:)+F); l(3,2) = norm(F-vert(4,:));
    M(4,4:6) = 1/2*(vert(2,:)+vert(3,:)); l(4,2) = norm(vert(2,:)-vert(3,:));
    M(5,4:6) = 1/2*(vert(2,:)+vert(4,:)); l(5,2) = norm(vert(2,:)-vert(4,:));
    M(6,4:6) = 1/2*(vert(3,:)+vert(4,:)); l(6,2) = norm(vert(3,:)-vert(4,:));
elseif size(intpt,1) == 4
    [intptU,~,~] = vert4to3(intpt);
    D = intptU(1,:); E = intptU(2,:); F = intptU(3,:); C = 1/3*(D+E+F);
    M(1,1:3) = 1/2*(vert(1,:)+vert(2,:)); l(1,1) = norm(vert(1,:)-vert(2,:));
    M(2,1:3) = 1/2*(vert(1,:)+intpt(1,:)); l(2,1) = norm(vert(1,:)-intpt(1,:));
    M(2,4:6) = 1/2*(vert(3,:)+intpt(1,:)); l(2,2) = norm(vert(3,:)-intpt(1,:));
    M(3,1:3) = 1/2*(vert(1,:)+intpt(2,:)); l(3,1) = norm(vert(1,:)-intpt(2,:));
    M(3,4:6) = 1/2*(vert(4,:)+intpt(2,:)); l(3,2) = norm(vert(4,:)-intpt(2,:));
    M(4,1:3) = 1/2*(vert(2,:)+intpt(3,:)); l(4,1) = norm(vert(2,:)-intpt(3,:));
    M(4,4:6) = 1/2*(vert(3,:)+intpt(3,:)); l(4,2) = norm(vert(3,:)-intpt(3,:));
    M(5,1:3) = 1/2*(vert(2,:)+intpt(4,:)); l(5,1) = norm(vert(2,:)-intpt(4,:));
    M(5,4:6) = 1/2*(vert(4,:)+intpt(4,:)); l(5,2) = norm(vert(4,:)-intpt(4,:));
    M(6,4:6) = 1/2*(vert(3,:)+vert(4,:)); l(6,2) = norm(vert(3,:)-vert(4,:));
end

t = zeros(6,6);
t(1,1:3) = vert(2,:)-vert(1,:); t(1,1:3) = t(1,1:3)/norm(t(1,1:3));
t(2,1:3) = vert(3,:)-vert(1,:); t(2,1:3) = t(2,1:3)/norm(t(2,1:3));
t(3,1:3) = vert(4,:)-vert(1,:); t(3,1:3) = t(3,1:3)/norm(t(3,1:3));
t(4,1:3) = vert(3,:)-vert(2,:); t(4,1:3) = t(4,1:3)/norm(t(4,1:3));
t(5,1:3) = vert(4,:)-vert(2,:); t(5,1:3) = t(5,1:3)/norm(t(5,1:3));
t(6,1:3) = vert(4,:)-vert(3,:); t(6,1:3) = t(6,1:3)/norm(t(6,1:3));

n = cross(E-D,F-D);
n = n/norm(n); % unit normal of plane DEF
% L = @(x,y,z) dot([x-C(1),y-C(2),z-C(3)],n);

%% for the piece2
bas0 = zeros(6,6);
bas0(:,1) = (M(:,2).*t(:,3) - M(:,3).*t(:,2)).*l(:,1) + (M(:,5).*t(:,3) - M(:,6).*t(:,2)).*l(:,2);
bas0(:,2) = (M(:,3).*t(:,1) - M(:,1).*t(:,3)).*l(:,1) + (M(:,6).*t(:,1) - M(:,4).*t(:,3)).*l(:,2);
bas0(:,3) = (M(:,1).*t(:,2) - M(:,2).*t(:,1)).*l(:,1) + (M(:,4).*t(:,2) - M(:,5).*t(:,1)).*l(:,2);
bas0(:,4:6) = t(:,1:3).*((l(:,1)+l(:,2))*ones(1,3));

%% for the piece1
MM = M - [ones(6,1)*C, ones(6,1)*C];
bas1 = zeros(6,6);
bas1(:,1) = ((ra-1)*(MM(:,2).*t(:,3) - MM(:,3).*t(:,2)) +...
    (rb-1)*(t(:,1:3)*n')*(C(2)*n(3)-C(3)*n(2))).*l(:,1);
bas1(:,2) = ((ra-1)*(MM(:,3).*t(:,1) - MM(:,1).*t(:,3)) +...
    (rb-1)*(t(:,1:3)*n')*(C(3)*n(1)-C(1)*n(3))).*l(:,1);
bas1(:,3) = ((ra-1)*(MM(:,1).*t(:,2) - MM(:,2).*t(:,1)) +...
    (rb-1)*(t(:,1:3)*n')*(C(1)*n(2)-C(2)*n(1))).*l(:,1);
bas1(:,1:3) = bas1(:,1:3) -...
    (ra-1)*(sum(cross(ones(6,1)*n,MM(:,1:3)).*t(:,1:3),2).*l(:,1))*n;
bas1(:,4:6) = (rb-1)*(l(:,1).*(t(:,1:3)*n'))*n;

A = bas0 + bas1;

ss = A\eye(6);
ss = ss';
BAS2 = ss;
BAS1 = ss;
BAS1(:,1:3) = ss(:,1:3) + (ra-1)*(ss(:,1:3)-(ss(:,1:3)*n')*n);
BAS1(:,4:6) = ss(:,4:6) - (ra-1)*cross(ss(:,1:3)-(ss(:,1:3)*n')*n,ones(6,1)*C) +...
    (rb-1)*((cross(ss(:,1:3),ones(6,1)*C)+ss(:,4:6))*n')*n;
%BAS1 = BAS1'; BAS2 = BAS2';

return;


% weighting
function [gen, weight_a, weight_b] = fusion_strategy(features_a, features_b, source_a, source_b)

[m1,n1] = size(source_a);
% resize
resize_temp1 = imresize(features_a, [m1, n1]);
resize_temp2 = imresize(features_b, [m1, n1]);
% soft-max
weight_ave_temp1 = resize_temp1./(resize_temp1+resize_temp2);
weight_ave_temp2 = resize_temp2./(resize_temp1+resize_temp2);
% reconstruction
gen = source_a.*weight_ave_temp1 + source_b.*weight_ave_temp2;
% figure;imshow(gen);

weight_a = weight_ave_temp1;
weight_b = weight_ave_temp2;
end


classdef MMF6 < PROBLEM
% <multi> <real> <multimodal>
% Multi-modal multi-objective test function

%------------------------------- Reference --------------------------------
% C. Yue, B. Qu, and J. Liang, A multi-objective particle swarm optimizer
% using ring topology for solving multimodal multiobjective Problems, IEEE
% Transactions on Evolutionary Computation, 2018, 22(5): 805-817.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        POS;    % Pareto optimal set for IGDX calculation
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            obj.D = 2;
            obj.lower    = [1,-1];
            obj.upper    = [3,2];
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            temp = X(:,2)<=0 | X(:,2)<=1 & (X(:,1)<=7/6|8/6<X(:,1)&X(:,1)<=9/6|10/6<X(:,1)&X(:,1)<=11/6|13/6<X(:,1)&X(:,1)<=14/6|15/6<X(:,1)&X(:,1)<=16/6|17/6<X(:,1));
            y    = zeros(size(X,1),1);
            y(temp)  = X(temp,2) - sin(6*pi*abs(X(temp,1)-2)+pi);
            y(~temp) = X(~temp,2) - 1 - sin(6*pi*abs(X(~temp,1)-2)+pi);
            PopObj(:,1) = abs(X(:,1)-2);
            PopObj(:,2) = 1 - sqrt(PopObj(:,1)) + 2*y.^2;
        end
        %% Generate Pareto optimal solutions
        function R = GetOptimum(obj,N)
            % Generate points in Pareto optimal set
            obj.POS(:,1) = linspace(1,3,N/2)';
            obj.POS(:,2) = sin(6*pi*abs(obj.POS(:,1)-2)+pi);
            obj.POS = [obj.POS;obj.POS(:,1),obj.POS(:,2)+1];
            % Generate points on Pareto front
            R(:,1) = linspace(0,1,N)';
            R(:,2) = 1 - sqrt(R(:,1));
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R(:,1) = linspace(0,1,100)';
            R(:,2) = 1 - sqrt(R(:,1));
        end
        %% Calculate the metric value
        function score = CalMetric(obj,metName,Population)
            switch metName
                case 'IGDX'
                    score = feval(metName,Population,obj.POS);
                otherwise
                    score = feval(metName,Population,obj.optimum);
            end
        end
        %% Display a population in the objective space
        function DrawObj(obj,Population)
            PopDec = Population.decs;
            temp1  = PopDec(:,2)<=0 | PopDec(:,2)<=1 & (PopDec(:,1)<=7/6|8/6<PopDec(:,1)&PopDec(:,1)<=9/6|10/6<PopDec(:,1)&PopDec(:,1)<=11/6|13/6<PopDec(:,1)&PopDec(:,1)<=14/6|15/6<PopDec(:,1)&PopDec(:,1)<=16/6|17/6<PopDec(:,1));
            temp2  = PopDec(:,1)<=2;
            Draw(Population(temp1&temp2).objs,'o','MarkerSize',6,'Marker','o','Markerfacecolor',[1 .5 .5],'Markeredgecolor',[1 .2 .2],{'\it f\rm_1','\it f\rm_2',[]});
            Draw(Population(temp1&~temp2).objs+0.05,'o','MarkerSize',6,'Marker','o','Markerfacecolor',[.5 .5 1],'Markeredgecolor',[.2 .2 1]);
            Draw(Population(~temp1&temp2).objs+0.1,'o','MarkerSize',6,'Marker','o','Markerfacecolor',[.5 1 .5],'Markeredgecolor',[.2 1 .2]);
            Draw(Population(~temp1&~temp2).objs+0.15,'o','MarkerSize',6,'Marker','o','Markerfacecolor',[1 .5 1],'Markeredgecolor',[1 .2 1]);
            Draw(obj.PF,'-','LineWidth',1,'Color',[1 .2 .2]);
            Draw(obj.PF+0.05,'-','LineWidth',1,'Color',[.2 .2 1]);
            Draw(obj.PF+0.1,'-','LineWidth',1,'Color',[.2 1 .2]);
            Draw(obj.PF+0.15,'-','LineWidth',1,'Color',[1 .2 1]);
        end
    end
end


function Data=FilterJumps(Dates,Data,Name)
Chgs=diff(Data);
DatesChgs=Dates(2:end);
ChgsNoJumps=Chgs;
DatesChgsNoJumps=DatesChgs;

Reject=find(abs(Chgs)>7*median(abs(Chgs)));  % reject outliers
ChgsNoJumps(Reject)=[];
DatesChgsNoJumps(Reject)=[];

ChgsJumps=0*DatesChgs;
ChgsJumps(Reject)=Chgs(Reject);

PlotSeries(DatesChgs,Chgs,DatesChgsNoJumps,ChgsNoJumps,ChgsJumps,Name)

Data=cumsum([ChgsNoJumps]);               % reconstruct series


clear
gcp;

name = 'granule_love2.tif';
if ~exist(name,'file')  % download file if it doesn't exist in the directory
    url = 'https://www.dropbox.com/s/mjmtwn4pdgydkny/granule_love2.tif.zip?dl=1';
    filename = 'granule_love2.tif.zip';
    fprintf('downloading the file...');
    outfilename = websave(filename,url);
    fprintf('...done. Now unzipping...')
    unzip(filename);
    fprintf('done.');
end

tic; Y = read_file(name); toc; % read the file (optional, you can also pass the path in the function instead of Y)
Y = single(Y);                 % convert to single precision
T = size(Y,ndims(Y));
Y = Y - min(Y(:));
%% set parameters (first try out rigid motion correction)

options_rigid = NoRMCorreSetParms('d1',size(Y,1),'d2',size(Y,2),'bin_width',200,'max_shift',15,'us_fac',50,'init_batch',200);

%% perform motion correction
tic; [M1,shifts1,template1,options_rigid] = normcorre(Y,options_rigid); toc

%% now try non-rigid motion correction (also in parallel)
options_nonrigid = NoRMCorreSetParms('d1',size(Y,1),'d2',size(Y,2),'grid_size',[32,32],'mot_uf',4,'bin_width',200,'max_shift',15,'max_dev',3,'us_fac',50,'init_batch',200);
tic; [M2,shifts2,template2,options_nonrigid] = normcorre_batch(Y,options_nonrigid); toc

%% compute metrics

nnY = quantile(Y(:),0.005);
mmY = quantile(Y(:),0.995);

[cY,mY,vY] = motion_metrics(Y,10);
[cM1,mM1,vM1] = motion_metrics(M1,10);
[cM2,mM2,vM2] = motion_metrics(M2,10);
T = length(cY);
%% plot metrics
figure;
    ax1 = subplot(2,3,1); imagesc(mY,[nnY,mmY]);  axis equal; axis tight; axis off; title('mean raw data','fontsize',14,'fontweight','bold')
    ax2 = subplot(2,3,2); imagesc(mM1,[nnY,mmY]);  axis equal; axis tight; axis off; title('mean rigid corrected','fontsize',14,'fontweight','bold')
    ax3 = subplot(2,3,3); imagesc(mM2,[nnY,mmY]); axis equal; axis tight; axis off; title('mean non-rigid corrected','fontsize',14,'fontweight','bold')
    subplot(2,3,4); plot(1:T,cY,1:T,cM1,1:T,cM2); legend('raw data','rigid','non-rigid'); title('correlation coefficients','fontsize',14,'fontweight','bold')
    subplot(2,3,5); scatter(cY,cM1); hold on; plot([0.9*min(cY),1.05*max(cM1)],[0.9*min(cY),1.05*max(cM1)],'--r'); axis square;
        xlabel('raw data','fontsize',14,'fontweight','bold'); ylabel('rigid corrected','fontsize',14,'fontweight','bold');
    subplot(2,3,6); scatter(cM1,cM2); hold on; plot([0.9*min(cY),1.05*max(cM1)],[0.9*min(cY),1.05*max(cM1)],'--r'); axis square;
        xlabel('rigid corrected','fontsize',14,'fontweight','bold'); ylabel('non-rigid corrected','fontsize',14,'fontweight','bold');
    linkaxes([ax1,ax2,ax3],'xy')
%% plot shifts

shifts_r = squeeze(cat(3,shifts1(:).shifts));
shifts_nr = cat(ndims(shifts2(1).shifts)+1,shifts2(:).shifts);
shifts_nr = reshape(shifts_nr,[],ndims(Y)-1,T);
shifts_x = squeeze(shifts_nr(:,1,:))';
shifts_y = squeeze(shifts_nr(:,2,:))';

patch_id = 1:size(shifts_x,2);
str = strtrim(cellstr(int2str(patch_id.')));
str = cellfun(@(x) ['patch # ',x],str,'un',0);

figure;
    ax1 = subplot(311); plot(1:T,cY,1:T,cM1,1:T,cM2); legend('raw data','rigid','non-rigid'); title('correlation coefficients','fontsize',14,'fontweight','bold')
            set(gca,'Xtick',[])
    ax2 = subplot(312); plot(shifts_x); hold on; plot(shifts_r(:,1),'--k','linewidth',2); title('displacements along x','fontsize',14,'fontweight','bold')
            set(gca,'Xtick',[])
    ax3 = subplot(313); plot(shifts_y); hold on; plot(shifts_r(:,2),'--k','linewidth',2); title('displacements along y','fontsize',14,'fontweight','bold')
            xlabel('timestep','fontsize',14,'fontweight','bold')
    linkaxes([ax1,ax2,ax3],'x')

%% plot a movie with the results

figure;
for t = 1:1:T
    subplot(121);imagesc(Y(:,:,t),[nnY,mmY]); xlabel('raw data','fontsize',14,'fontweight','bold'); axis equal; axis tight;
    title(sprintf('Frame %i out of %i',t,T),'fontweight','bold','fontsize',14); colormap('bone')
    subplot(122);imagesc(M2(:,:,t),[nnY,mmY]); xlabel('non-rigid corrected','fontsize',14,'fontweight','bold'); axis equal; axis tight;
    title(sprintf('Frame %i out of %i',t,T),'fontweight','bold','fontsize',14); colormap('bone')
    set(gca,'XTick',[],'YTick',[]);
    drawnow;
    pause(0.02);
end


function [ih,xa,ya]=imagehomog(im,h,m,clip)
%IMAGEHOMOG Apply a homography transformation to an image with bilinear interpolation
%Inputs: im(ny,nx,nc)  input image (uint8)
%        h(3,3)        homography
%        m             mode string
%                         i  show image [default if no output arguments]
%                         m  matlab coordinates (1,1) is top left [default]
%                         c  central coordinates (0,0) is the centre = {(1+nx)/2,(1+ny)/2} in 'm'
%                         k  clip to original image dimensions
%                         x  extend zero-fill to the specified clipping rectangle
%        clip(4)       bounding box [xmin xmax ymin ymax]
% Outputs:
%        ih(my,mx,nc)  output image (uint8)
%        xa(mx)        x axis
%        ya(my)        y axis

% Bugs/Suggestions:
% (1) cope with non-uint8
% (2) cope with (a) multiple inputs, (b) multiple transformations
% (3) output a boundary mask as an alpha channel
% (4) do anti-aliasing along the boundary
% (5) check that origin shift is correct for central coordinates

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

maxby=1e7;  % maximum memory to use
[ny,nx,nc]=size(im);
if nargin<4
    clip=[];
    if nargin<3
        m='';
        if nargin<2
            h=eye(3);
        end
    end
end
imr=reshape(im,nx*ny,nc);
t=eye(3);
if any(m=='c')   % convert homography and clipping box to matlab coordinates
    t(7:8)=0.5+[nx ny]/2;  % shift origin to image centre
    h=t*h/t;  % change homography so input and output use MATLAB coordinates
    if numel(clip)
        clip=clip+t([7 7 8 8]);  % make clipping use MATLAB coordinates as well
    end
end
box=h*[1 1 nx nx; 1 ny 1 ny; 1 1 1 1];
box=box(1:2,:)./box([3 3],:);
box=[min(box(1,:)) max(box(1,:)) min(box(2,:)) max(box(2,:))];
if any(m=='k')
    clip=[1 nx 1 ny];
end
if ~numel(clip)
    clip=box;
end
clip=clip(:)';
clip(1:2:3)=floor(clip(1:2:3));
clip(2:2:4)=ceil(clip(2:2:4));
box(1:2:3)=floor(max(clip(1:2:3),box(1:2:3)));  % no point in calculating non-existant points
box(2:2:4)=ceil(min(clip(2:2:4),box(2:2:4)));
g=inv(h);
mx=box(2)-box(1)+1; % number of columns in destination
my=box(4)-box(3)+1; % number of rows in destination

ih=zeros(my*mx,nc,'uint8');
ncol=max(1,min(mx,floor(maxby/(my*nc*8)))); % number of columns to do in a chunk
nloop=ceil(mx/ncol);
cmax=1+rem(mx-1,ncol); % final column of first iteration
jxinc=ncol*my;      % increment target indices each loop
ginc=g(:,1)*ncol; % increment transformed targets each loop
wj=ones(1,jxinc);   % repeat index for ginc
jx=1+cmax*my-jxinc:cmax*my;  % initial target indices (some might be negative)
gk=g*[reshape(repmat(cmax-ncol+box(1):cmax+box(1)-1,my,1),1,jxinc); repmat(box(3):box(4),1,ncol); ones(1,jxinc)];
gn=gk(1:2,:)./gk([3 3],:);   % normalize source coordinates
mn=[zeros(1,jxinc-cmax*my) ones(1,cmax*my)]; % mask for initial iteration
mn=mn & (gn(1,:)>-0.5 & gn(2,:)>-0.5 & gn(1,:)<nx+0.5 & gn(2,:)<ny+0.5); % mask valid pixels
w3=ones(nc,1);
for i=1:nloop
    fn=max(floor(gn(:,mn)),1);
    fn1=min(max(fn(1,:)',1),nx-1);
    fn2=min(max(fn(2,:)',1),ny-1);
    dn=gn(:,mn)-[fn1 fn2]';
    dn1=min(max(dn(1,:)',0),1);
    dn2=min(max(dn(2,:)',0),1);
    dn1c=1-dn1;
    dn2c=1-dn2;
    ih(jx(mn),:)=uint8(dn1c(:,w3).*(dn2c(:,w3).*single(imr(fn2+ny*(fn1-1),:)) ...
        +dn2(:,w3).*single(imr(fn2+1+ny*(fn1-1),:))) ...
        +dn1(:,w3).*(dn2c(:,w3).*single(imr(fn2+ny*(fn1),:)) ...
        +dn2(:,w3).*single(imr(fn2+1+ny*(fn1),:))));
    jx=jx+jxinc;  % target indices
    gk=gk+ginc(:,wj);
    gn=gk(1:2,:)./gk([3 3],:);   % normalize source coordinates
    mn=gn(1,:)>-0.5 & gn(2,:)>-0.5 & gn(1,:)<nx+0.5 & gn(2,:)<ny+0.5; % mask valid pixels
end
ih=reshape(ih,[my,mx,nc]);
if any(m=='x')          % extend blank area to specified clipping rectangle
    ih=[zeros(box(3)-clip(3),clip(2)-clip(1)+1,nc,'uint8'); ...
        zeros(my,box(1)-clip(1),nc,'uint8') ih zeros(my,clip(2)-box(2),nc,'uint8');
        zeros(clip(4)-box(4),clip(2)-clip(1)+1,nc,'uint8')];
    xa=(clip(1):clip(2))-t(7);
    ya=(clip(3):clip(4))-t(8);
else
    xa=(box(1):box(2))-t(7);
    ya=(box(3):box(4))-t(8);
end

if ~nargout || any(m=='i')
    imagesc(xa,ya,ih);
    axis equal
end


function test_pull1427

% MEM 6gb
% WALLTIME 1:00:00
% DEPENDENCY ft_prepare_mesh ft_prepare_headmodel

%%

% This function creates an hexahedral and tetrahedral volumetric mesh from a
% three-compartments volume to be passed as input to ft_prepare_headmodel with
% the method 'simbio'.

segprob = [];
segprob.brain = false(10,10,10); segprob.brain(4:7,4:7,4:7) = true;
segprob.skull = false(10,10,10); segprob.skull(3:8,3:8,3:8) = true;
segprob.scalp = false(10,10,10); segprob.scalp(2:9,2:9,2:9) = true;
% segprob.air = true(10,10,10);
% segprob.air(2:9,2:9,2:9) = false;
%segprob.brain = false(11,11,11); segprob.brain(4:8,4:8,4:8) = true;
%segprob.skull = false(11,11,11); segprob.skull(3:9,3:9,3:9) = true;
%segprob.scalp = false(11,11,11); segprob.scalp(2:10,2:10,2:10) = true;
segprob.dim = size(segprob.brain);
segprob.unit = 'cm';
segprob.coordsys = 'ctf';
segprob.transform = eye(4);
segprob.transform(1,4) = -0.5;
segprob.transform(2,4) = -0.5;
segprob.transform(3,4) = -0.5;

% it is more difficult to visualize a probabilistic segmentation than an indexed one
segindx = ft_datatype_segmentation(segprob, 'segmentationstyle', 'indexed');

cfg = [];
cfg.funparameter = 'tissue';
cfg.method = 'ortho';
cfg.location = [5 5 5]; % this is the center of the volume, in this plot it will be rounded off to the nearest voxel
ft_sourceplot(cfg, segindx);

% determine the range of the bounding box
[X, Y, Z] = ndgrid(1:segprob.dim(1), 1:segprob.dim(2), 1:segprob.dim(3));
voxpos = ft_warp_apply(segprob.transform, [X(:) Y(:) Z(:)]);
minmaxpos(:,1) = min(voxpos) - 0.5;
minmaxpos(:,2) = max(voxpos) + 0.5;

%%

cfg = [];
cfg.shift = 0.3;
cfg.method = 'hexahedral';
mesh_vol_hex1 = ft_prepare_mesh(cfg, segprob);

figure
ft_plot_ortho(segindx.tissue, 'transform', segindx.transform, 'location', [5 5 5], 'style', 'intersect');
hold on
ft_plot_mesh(mesh_vol_hex1, 'surfaceonly', false, 'facecolor', 'none', 'edgecolor', 'm');
view(120, 30)

cfg = [];
cfg.method ='simbio';
cfg.conductivity = [0.33, 0.43, 0.53]; % order follows mesh.tissuelabel
ft_prepare_headmodel(cfg, mesh_vol_hex1)

%%

% make a volume with 2x the resolution
cfg = [];
cfg.resolution = 1/2;
cfg.xrange     = minmaxpos(1,:); % this is more robust than the dim, in case the origin is not inside the volume
cfg.yrange     = minmaxpos(2,:);
cfg.zrange     = minmaxpos(3,:);
cfg.method     = 'nearest';
segprob2 = ft_volumereslice(cfg, segprob);
segindx2 = ft_volumereslice(cfg, segindx);

cfg = [];
cfg.shift = 0.3;
cfg.method = 'hexahedral';
mesh_vol_hex2 = ft_prepare_mesh(cfg, segprob2);

% this one looks OK in terms of alignment
figure
% ft_plot_ortho(segindx.tissue, 'transform', segindx.transform, 'location', [5 5 5], 'style', 'intersect');
ft_plot_ortho(segindx2.tissue, 'transform', segindx2.transform, 'location', [5 5 5], 'style', 'intersect'); % this one looks OK in terms of alignment
hold on
ft_plot_mesh(mesh_vol_hex2, 'surfaceonly', false, 'facecolor', 'none', 'edgecolor', 'm');
view(120, 30)

cfg = [];
cfg.method ='simbio';
cfg.conductivity = [0.33, 0.43, 0.53]; % order follows mesh.tissuelabel
ft_prepare_headmodel(cfg, mesh_vol_hex1)

%%

% make a volume with 3x the resolution
cfg = [];
cfg.resolution = 1/3;
cfg.xrange     = minmaxpos(1,:); % this is more robust than the dim, in case the origin is not inside the volume
cfg.yrange     = minmaxpos(2,:);
cfg.zrange     = minmaxpos(3,:);
cfg.method     = 'nearest';
segprob3 = ft_volumereslice(cfg, segprob);
segindx3 = ft_volumereslice(cfg, segindx);

cfg = [];
cfg.shift = 0.3;
cfg.method = 'hexahedral';
mesh_vol_hex3 = ft_prepare_mesh(cfg, segprob3);

figure
% ft_plot_ortho(segindx.tissue, 'transform', segindx.transform, 'location', [5 5 5], 'style', 'intersect');
% ft_plot_ortho(segindx2.tissue, 'transform', segindx2.transform, 'location', [5 5 5], 'style', 'intersect');
ft_plot_ortho(segindx3.tissue, 'transform', segindx3.transform, 'location', [5 5 5], 'style', 'intersect'); % this one looks OK in terms of alignment
hold on
ft_plot_mesh(mesh_vol_hex3, 'surfaceonly', false, 'facecolor', 'none', 'edgecolor', 'm');
view(120, 30)

cfg = [];
cfg.method ='simbio';
cfg.conductivity = [0.33, 0.43, 0.53]; % order follows mesh.tissuelabel
ft_prepare_headmodel(cfg, mesh_vol_hex3)

%%

cfg = [];
cfg.method = 'tetrahedral';
mesh_vol_tet = ft_prepare_mesh(cfg, segprob);

figure
ft_plot_ortho(segindx.tissue, 'transform', segindx.transform, 'location', [5 5 5], 'style', 'intersect');
hold on
ft_plot_mesh(mesh_vol_tet, 'surfaceonly', false, 'facecolor', 'none', 'edgecolor', 'm');
view(120, 30)

cfg = [];
cfg.method ='simbio';
cfg.conductivity = [0.33, 0.43, 0.53]; % order follows mesh.tissuelabel
ft_prepare_headmodel(cfg, mesh_vol_tet)


addpath('..\mexopencv\');
dir='G:\data\20130808\109ND800_080813\';
prefix=sprintf('%sDSC_', dir);

img0 = takeImage_v002(prefix, 1023, 'jpg', 3);
mask=uint8(ones(size(img0,1),size(img0,2)));
mask(:,:)=1;
excluded_band=100;
mask(1:excluded_band,:)=0;
mask(end-excluded_band+1:end,:)=0;
mask(:,1:excluded_band)=0;
mask(:,end-excluded_band+1:end)=0;

 prevPts = cv.goodFeaturesToTrack(img0,'MaxCorners', 200, ...
 'QualityLevel', 0.02,'MinDistance', 10,'Mask', mask, 'BlockSize', 3, 'UseHarrisDetector', 0, 'K',0.04);
prevPts=cv.cornerSubPix(img0, prevPts);

% the argument order and vacancy does not matter in matlab in constrast to
% opencv C++
corners=zeros(2,size(prevPts,2));
    for jerk=1:size(prevPts,2)
        if(~isempty(prevPts{jerk}))
   corners(:,jerk)= prevPts{jerk}';
        end
    end

% increase lambda apparently increase number of corners
%larger gaussian window sz2 will slightly decrease the number of corners

figure(1); clf;
imshow(img0);
hold on;            %# Add subsequent plots to the image
%plot(corners(2,:),corners(1,:),'go');  %# NOTE: x_p and y_p are switched (see note below)!
plot(corners(1,:),corners(2,:),'ro','MarkerSize',5);

hold off;
[m,n,p]=size(img0);
winsz1=7;
sigma1=10;

imglast=img0(:,:,1);
for i=1024:1055
    im=takeImage_v002(prefix,i, 'jpg', 3);
            maxedge=max(size(im));
                while(maxedge>1000)
                    im=impyramid(im, 'reduce');% 8UC1
                    maxedge=round(maxedge/2);
                end
%     img= double((read(myVid,i-1)));
%     img2=double((read(myVid,i)));
%     for band=1:size(img,3)
%         img(:,:,band)=simgauss(img(:,:,band), winsz1, sigma1);
%         img2(:,:,band)=simgauss(img2(:,:,band), winsz1, sigma1);
%     end
    [prevPts, status, err] = cv.calcOpticalFlowPyrLK(imglast, im, prevPts, 'WinSize', [23, 23], ...
        'MaxLevel', 2,'Criteria', struct('type', 'Count+EPS', 'maxCount', 20, 'epsilon', 0.01));
    for jerk=1:length(status)
        if(status~=0)
        end
    end
    corners=zeros(2,size(prevPts,2));
    for jerk=1:size(prevPts,2)
        if(~isempty(prevPts{jerk}))
   corners(:,jerk)= prevPts{jerk}';
        end
    end

   % corners=SimpKLT(img,img2,corners,3);
    imshow(im);
    hold on
    plot(corners(1,:),corners(2,:),'ro','MarkerSize',5);
    hold off
    drawnow;
   imglast=im;

end

% close(writerObj);


% absolute change in body orientation
function [data,units] = compute_absdtheta(trx,n)

flies = trx.exp2flies{n};
nflies = numel(flies);
data = cell(1,nflies);
for i = 1:nflies,
  fly = flies(i);
  data{i} = abs(trx(fly).dtheta);
end
units = parseunits('rad/s');


% Copyright 2011 Zdenek Kalal
%
% This file is part of TLD.
%
% TLD 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.
%
% TLD 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 TLD.  If not, see <http://www.gnu.org/licenses/>.

function patch = img_patch(img, bb, randomize,p_par)

if nargin == 4 && randomize > 0

    rand('state',randomize);
    randn('state',randomize);

    NOISE = p_par.noise;
    ANGLE = p_par.angle;
    SCALE = p_par.scale;
    SHIFT = p_par.shift;

    cp  = bb_center(bb)-1;
    Sh1 = [1 0 -cp(1); 0 1 -cp(2); 0 0 1];

    sca = 1-SCALE*(rand-0.5);
    Sca = diag([sca sca 1]);

    ang = 2*pi/360*ANGLE*(rand-0.5);
    ca = cos(ang);
    sa = sin(ang);
    Ang = [ca, -sa; sa, ca];
    Ang(end+1,end+1) = 1;

    shR  = SHIFT*bb_height(bb)*(rand-0.5);
    shC  = SHIFT*bb_width(bb)*(rand-0.5);
    Sh2 = [1 0 shC; 0 1 shR; 0 0 1];

    bbW = bb_width(bb)-1;
    bbH = bb_height(bb)-1;
    box = [-bbW/2 bbW/2 -bbH/2 bbH/2];

    H     = Sh2*Ang*Sca*Sh1;
    bbsize = bb_size(bb);
    patch = uint8(warp(img,inv(H),box) + NOISE*randn(bbsize(1),bbsize(2)));

else

    % All coordinates are integers
    if isempty(find((round(bb)-bb) ~= 0, 1)) == 1
        L = max([1 bb(1)]);
        T = max([1 bb(2)]);
        R = min([size(img,2) bb(3)]);
        B = min([size(img,1) bb(4)]);
        patch = img(T:B,L:R);

        % Sub-pixel accuracy
    else

        cp = 0.5 * [bb(1)+bb(3); bb(2)+bb(4)]-1;
        H = [1 0 -cp(1); 0 1 -cp(2); 0 0 1];

        bbW = bb(3,:)-bb(1,:);
        bbH = bb(4,:)-bb(2,:);
        if bbW <= 0 || bbH <= 0
            patch = [];
            return;
        end
        box = [-bbW/2 bbW/2 -bbH/2 bbH/2];

        if size(img,3) == 3
            for i = 1:3
                P = warp(img(:,:,i),inv(H),box);
                patch(:,:,i) = uint8(P);
            end
        else
            patch = warp(img,inv(H),box);
            patch = uint8(patch);
        end

    end
end


% [INPUT]
% r = A vector of floats (-Inf,Inf) of length t representing the logarithmic returns.
% v = A vector of floats [0,Inf) of length t representing the trading volumes.
% sv = A float t-by-k matrix (-Inf,Inf) representing the state variables.
% bw = An integer [21,252] representing the dimension of each rolling window.
% mem = A string representing the MEM type:
%   - 'B' for Baseline MEM;
%   - 'A' for Asymmetric MEM;
%   - 'P' for Asymmetric Power MEM;
%   - 'S' for Spline MEM.
% mag = An integer [1,Inf) obtained as 10^x representing the magnitude of logarithmic returns and trading volumes (optional, default=[]).
%
% [OUTPUT]
% illiq = A column vector of floats [0,Inf) of length t representing the ILLIQ indicator.
% illiqc = A column vector of floats [0,Inf) of length t representing the ILLIQ indicator with covariates if state variables are provided, an empty array otherwise.
% knots = A row vector of floats [0,Inf) containing the optimal number of knots if Spline MEM is used, an empty array otherwise.

function [illiq,illiqc,knots] = illiq_indicator(varargin)

    persistent ip;

    if (isempty(ip))
        ip = inputParser();
        ip.addRequired('r',@(x)validateattributes(x,{'double'},{'real' 'finite' 'vector' 'nonempty'}));
        ip.addRequired('v',@(x)validateattributes(x,{'double'},{'real' 'finite' 'nonnegative' 'vector' 'nonempty'}));
        ip.addRequired('sv',@(x)validateattributes(x,{'double'},{'real' 'finite'}));
        ip.addRequired('bw',@(x)validateattributes(x,{'double'},{'real' 'finite' 'integer' '>=' 21 '<=' 252 'scalar'}));
        ip.addRequired('mem',@(x)any(validatestring(x,{'A' 'B' 'P' 'S'})));
        ip.addOptional('mag',[],@(x)validateattributes(x,{'double'},{'real' 'finite' 'integer'}));
    end

    ip.parse(varargin{:});

    ipr = ip.Results;
    [r,v,sv,mag] = validate_input(ipr.r,ipr.v,ipr.sv,ipr.mag);
    bw = ipr.bw;
    mem = ipr.mem;

    nargoutchk(2,3);

    [illiq,illiqc,knots] = illiq_indicator_internal(r,v,sv,bw,mem,mag);

end

function [illiq,illiqc,knots] = illiq_indicator_internal(r,v,sv,bw,mem,mag)

    alpha = 2 / (bw + 1);

    input = mag .* (abs(r) ./ v);
    input(~isfinite(input) | (input == 0)) = NaN;
    input(isnan(input)) = mean(input,'omitnan');

    if (any(strcmp(mem,{'A' 'P'})))
        input = [input r];
    end

    knots = [];

    [illiq,~,mem_params] = multiplicative_error(input,mem);
    illiq = [illiq(1); filter(alpha,[1 (alpha - 1)],illiq(2:end),(1 - alpha) * illiq(1))];
    illiq = (illiq - min(illiq)) ./ (max(illiq) - min(illiq));

    if (strcmp(mem,'S'))
        knots(1) = mem_params(1);
    end

    if (isempty(sv))
        illiqc = [];
    else
        [illiqc,~,mem_params] = multiplicative_error([input sv],mem);
        illiqc = [illiqc(1); filter(alpha,[1 (alpha - 1)],illiqc(2:end),(1 - alpha) * illiqc(1))];
        illiqc = (illiqc - min(illiqc)) ./ (max(illiqc) - min(illiqc));

        if (strcmp(mem,'S'))
            knots(2) = mem_params(1);
        end
    end

end

function [r,v,sv,mag] = validate_input(r,v,sv,mag)

    data = {r(:) v(:)};

    l = unique(cellfun(@numel,data));

    if (numel(l) ~= 1)
        error('The number of elements of ''r'' and ''v'' must be equal.');
    end

    if (l < 5)
        error('The value of ''r'' and ''v'' is invalid. Expected inputs to be vectors containing at least 5 elements.');
    end

    [r,v] = deal(data{:});

    if (~isempty(sv))
        if (size(sv,1) ~= l)
            error(['The value of ''sv'' is invalid. Expected input to be a matrix with ' num2str(l) ' rows.']);
        end
    end

    if (isempty(mag))
        mag_r = floor(round((log(abs(r)) ./ log(10)),15));
        mag_r(~isfinite(mag_r)) = [];
        mag_r = round(abs(mean(mag_r)),0);

        mag_v = floor(round((log(abs(v)) ./ log(10)),15));
        mag_v(~isfinite(mag_v)) = [];
        mag_v = round(mean(mag_v),0);

        mag = 10^(mag_r + mag_v);
    else
        if ((mag ~= 1) && (rem(mag,10) ~= 0))
            error('The value of ''mag'' is invalid. Expected input to be an integer obtained as 10^x.');
        end
    end

end


function y = inter_xls(Y,x,ta,sc,type,ip,d,flax1,flax2,file_name)
% PURPOSE: Interface via Excel Link for univariate temporal disaggregation
% -----------------------------------------------------------------------
% SYNTAX: y = inter_xls(Y,x,ta,sc,type,ip,d,flax1,flax2,file_name);
% -----------------------------------------------------------------------
% INPUT
%           SELECTION OF THE METHOD
%
% Common parameters:
%        ta: type of disaggregation
%            ta=1 ---> sum (flow)
%            ta=2 ---> average (index)
%            ta=3 ---> last element (stock) ---> interpolation
%            ta=4 ---> first element (stock) ---> interpolation
%        sc: number of high frequency data points for each low frequency data point
%            sc= 4 ---> annual to quarterly
%            sc=12 ---> annual to monthly
%            sc= 3 ---> quarterly to monthly
%
% Specific parameters:
%
% ==> Boot-Feibes-Lisman, Denton (additive variant, standard solution):
%        d: objective function to be minimized: volatility of ...
%            d=0 ---> levels (only BFL)
%            d=1 ---> first differences
%            d=2 ---> second differences
%
% ==> Chow-Lin, Fernandez, Litterman, Santos-Cardoso:
%           opC = -1 ---> pretesting intercept signficiance
%
% ==> Chow-Lin, Litterman, Santos-Cardoso:
%        type: estimation method:
%            type=0 ---> weighted least squares
%            type=1 ---> maximum likelihood
%        innovational parameter rl = []. Default: range [.05 .99], 100 points grid.
%
% ==> Chow-Lin, Litterman, Santos-Cardoso when innovational parameter is supplied:
%         ip, such as -1 < ip < 1
%
%           INPUT DATA:
%
% Common:
%       Y: Nx1 --> Low-frequency time series (to be temporally disaggregated)
% Specific:
%       x: nx1 --> Denton, n=sc*N (extrapolation is not feasible)
%       x: nxp --> Fernandez, Chow-Lin, Litterman, Santos-Cardoso
%          p >= 1, n >= sc*N (extrapolation is feasible)
%
% -----------------------------------------------------------------------
% OUTPUT: y: nxi
%
%       i=1 brief --> only temporally disaggregated series (all procedures)
%       i=5 normal --> temporally disaggregated series, standard errors of estimates,
%                  one-sigma upper and lower limits and residuals.
%                  Available for Fernandez, Chow-Lin, Litterman, Santos-Cardoso
%       i=5 detailed --> normal + ASCII file with model results (all
%                   procedures). A name for the output file should be supplied.
%
% -----------------------------------------------------------------------
% LIBRARY: bfl, denton_uni, fernandez, chowlin, litterman, ssc,
% td_uni_print, td_print

% written by:
% Ana Abad(*) & Enrique M. Quilis(**)
%   (*) National Statistical Institute
%   (**) Macroeconomic Research Department
%        Ministry of Economy and Competitiveness
%        <enrique.quilis@mineco.es>

% Version 2.3 (January, 2013)

% -----------------------------------------------------------------------
% SELECTION OF THE METHOD

% Intercept pretesting
opC = -1;

% Range for grid search: default case: search is performed
% on the range [.05 .99], 100 points grid. Default.
rl = [];

switch flax1
    case 1
        % Boot-Feibes-Lisman
        res=bfl(Y,ta,d,sc);
    case 2
        % Denton
        op1 = 1; %Additive variant
        if (d == 0); d=1; end; %New version requires d=1 or d=2.
        res=denton_uni(Y,x,ta,d,sc,op1);
    case 3
        % Fernandez
         res=fernandez(Y,x,ta,sc,opC);
     case 4
         % Chow-Lin
         res=chowlin(Y,x,ta,sc,type,opC,rl);
     case 5
         % Litterman
         res=litterman(Y,x,ta,sc,type,opC,rl);
     case 6
         % Santos-Cardoso
         res=ssc(Y,x,ta,sc,type,opC,rl);
     case 7
         % Chow-Lin, fixed innovational parameter (ip)
         res=chowlin(Y,x,ta,sc,type,opC,ip);
     case 8
         % Litterman, fixed innovational parameter (ip)
         res=litterman(Y,x,ta,sc,type,opC,ip);
     case 9
         % Santos-Cardoso, fixed innovational parameter (ip)
         res=ssc(Y,x,ta,sc,type,opC,ip);
  end

% -----------------------------------------------------------------------
% SELECTION OF OUTPUT

 switch flax2
     case 1
         % Brief output
         y = res.y;
     case 2
         % Normal output
         switch res.meth
             case {'Boot-Feibes-Lisman'}
                 y = [res.y zeros(res.sc*res.N,4)];
             case {'Denton'}
                  y = [res.y zeros(res.sc*res.N,3) res.u];
             case {'Fernandez','Chow-Lin','Litterman','Santos Silva-Cardoso'}
                 y = [res.y res.y_dt res.y_lo res.y_up res.u];
         end
     case 3
         % Detailed output
         switch res.meth
             case {'Boot-Feibes-Lisman'}
                 y = [res.y zeros(res.sc*res.N,4)];
                 tduni_print(res,file_name);
             case {'Denton'}
                  y = [res.y zeros(res.sc*res.N,3) res.u];
                  tduni_print(res,file_name);
             case {'Fernandez','Chow-Lin','Litterman','Santos Silva-Cardoso'}
                 y = [res.y res.y_dt res.y_lo res.y_up res.u];
                 td_print(res,file_name);
         end
 end


function view = saveGlmMap(view, fieldName, condNum, mapName, scan)
%
% view = saveGlmMap(view, fieldName, condNum, [mapName], [scan]);
%
% Load a GLM model for a mrVista view (saved for the specified scan),
% and save the specifield field name as a parameter map.
%
% To use this, you should have run 'applyGlm' on the view, to save
% the GLM data files.
%
% ras, 1/03/06
if ieNotDefined('view'), view = getSelectedInplane; end
if ieNotDefined('mapName'), mapName = fieldName; end
if ieNotDefined('scan'), scan = getCurScan(view); end

% initialize map as zeros
map = cell(1, numScans(view));
mapSize = dataSize(view);
map{scan} = zeros(mapSize);

h = mrvWaitbar(0, 'Saving GLM Map...');

for slice = 1:numSlices(view)
   % load the results of the GLM
   model = loadGlmSlice(view, slice, scan);

   % find the indices in the map which correspond to this slice
   [X Y Z] = meshgrid(1:mapSize(1), 1:mapSize(2), slice);
   coords = [X(:) Y(:) Z(:)]';
   ind = sub2ind(mapSize, coords(1,:), coords(2,:), coords(3,:));

   % get the data from the appropriate field
   if ~isfield(model, fieldName)
       error('field not found')
   elseif isequal(lower(fieldName), 'residual')
       % take the mean residual from each voxel
       vals = mean(model.(fieldName), 1);
   else
       vals = model.(fieldName)(:, condNum, :);
   end

   % map the values from the specified field to the appropriate
   % place in the map:
   map{scan}(ind) = vals;

   mrvWaitbar(slice/numSlices(view), h);
end

close(h);

% set the absolute value of the parameter as the 'co' field, so
% you can threshold by that as well:
co = cell(1, numScans(view));
co{scan} = abs(map{scan});

% save the map
mapPath = fullfile(dataDir(view), mapName);
save(mapPath, 'map', 'co', 'mapName');

% set as the active map in the view, so you can view the
% results right away:
view = setParameterMap(view, map, mapName);

return


function mask = triangle_mask ( dim_num, triangle_order, nodes, coord )

%*****************************************************************************80
%
%% TRIANGLE_MASK is a user routine which masks triangles.
%
%  Discussion:
%
%    The region to be considered is the [0,4]x[0,4] square.
%
%    We want to remove the lower left triangular corner,
%    and part of the upper right triangular corner.
%
%    The following diagram of the 25 nodes indicates by "O" the
%    nodes that should end up being deleted, although the deletion
%    is actually done by triangles.
%
%    Before masking:
%
%      X - X - X - X - X
%      | \ | \ | \ | \ |
%      X - X - X - X - X
%      | \ | \ | \ | \ |
%      X - X - X - X - X
%      | \ | \ | \ | \ |
%      X - X - X - X - X
%      | \ | \ | \ | \ |
%      X - X - X - X - X
%
%    After masking:
%
%      X - X   O   O   O
%      | \ | \
%      X - X - X   O   O
%      | \ | \ | \
%      X - X - X - X - X
%        \ | \ | \ | \ |
%      O   X - X - X - X
%            \ | \ | \ |
%      O   O   X - X - X
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer TRIANGLE_ORDER, the number of nodes in the triangle.
%
%    Input, integer NODES(TRIANGLE_ORDER), the indices of the nodes.
%
%    Input, real COORD(DIM_NUM,TRIANGLE_ORDER), the coordinates
%    of the nodes.
%
%    Output, logical MASK, is TRUE if the triangle should be discarded,
%    and FALSE if the triangle should be retained.
%

%
%  Compute the centroid.
%
  for dim = 1 : dim_num
    centroid(dim) = sum ( coord(dim,1:triangle_order) ) / triangle_order;
  end
%
%  Remove the lower left corner
%
  if ( centroid(1) + centroid(2) < 2.0 )

    mask = 1;
%
%  Remove the upper right section.
%
  elseif ( 5.0 < centroid(1) + centroid(2) & 2.0 < centroid(2) )

    mask = 1;
%
%  Keep everything else.
%
  else

    mask = 0;

  end

  return
end


% This script depends on
% - ex_subspace_angles.m
% - ex_omp_rep_subspace_angles.m
% Please execute the dependencies before
% running this script.

close all; clear all; clc;

export = true;

orig = load('bin/principal_angles');
mf = spx.graphics.Figures();
mf.new_figure('Original Principal angles');
hist(orig.off_diag_angles, 200);
xlabel('Principal angle (degrees)');
ylabel('Number of subspace pairs');
title('Distribution of principal angles over subspace pairs in signal space');
grid on;

if export
export_fig bin/images/signal_space_principal_angles.png -r120 -nocrop;
export_fig bin/images/signal_space_principal_angles.pdf;
end

rep = load('bin/omp_rep_principal_angles');
mf.new_figure('Representation Principal angles');
hist(rep.off_diag_angles, 200);
xlabel('Principal angle (degrees)');
ylabel('Number of subspace pairs');
title('Distribution of principal angles over subspace pairs in representations');
grid on;

if export
export_fig bin/images/representation_space_principal_angles.png -r120 -nocrop;
export_fig bin/images/representation_space_principal_angles.pdf;
end


function res = matrix_max_percent(mat)
    res = floor(mat/max(mat(:))*100);
end


function [Mean_accuracy, Variance,p1,m1,Means,Vars,Maxsf,Minsf]=simo2(data,v,c, measure, p, m, N,rn,pl)
%Inputs:
% data: put your data file in matrix form, rows indicates samples and
% columns features of the data.
% v:   how many columns of features you are using e.g. [1:4] means you will
% use first four columns as features of your data.
% c: column where you class labels are. They should be in numerical form.
% measure: Which quantifier in OWA operators you want to use.
% 1=Basic RIM quantifier
% 2=Quadratic quantifier
% 3=Exponential quantifier
% 4=Trigonometric quantifier
% 5=O'Hagans method.
% p: parameter in generalized Lukasiewics similarity, can be studied as a range of
% parameter values and then given as a vector i.e. p=[0.1:0.1:5].
% alpha: Alpha value in owa operators
% Can be given as vector i.e. m=[0.1:0.1:5].
% N: how many times data is divided randomly into testing set and learning
% set.
% rn: portion of data which is used in learning set. default rn=1/2 (data
% is divided in half; half is used in testing set and half in learning set.
%pl: do you want to plot how the parameter changes in p and m changes the
% mean classification accuracies and variances.
%
%OUTPUTS:
% Mean_accuracy: Mean classification accuracy with best parameter values in
% p and m:
% p1 and m1: best parameter values w.r.t. mean classification accuracy.
% Variance: Variances with best parameter values

sv_name = 'results2'; % mat file where results are stored ('' = "no storing")

[data, lc, cs] = init_data(data,v,c); % data initialization

w_opt = 0; % Do we use weight optimization. Not implemented in this version.

%Initializations
fitness = zeros(1,N);
fitness_id = zeros(1,N);
fitness_dif = zeros(1,N);
Means = zeros(length(p),length(m),length(rn));
Vars = zeros(length(p),length(m),length(rn));
Maxsf = zeros(length(p),length(m),length(rn));
Minsf = zeros(length(p),length(m),length(rn));

Means_fit_dif = zeros(length(p),length(m),length(rn));
Vars_fit_dir = zeros(length(p),length(m),length(rn));
Ideal_var = zeros(length(p),length(m), length(lc),length(rn));

for n = 1 : length(rn)
    rn_ideal = ones(1,length(lc))*rn(n);
    for j = 1:length(m)
        for i = 1:length(p)
            y = [p(i), m(j),measure]; % p and m values and similarity measure
            for k = 1 : N
                ideal_ind = [];
                for l = 1 :length(lc)
                    temp = randperm(lc(l))-1;
                    ideal_ind = [ideal_ind, cs(l)+temp(1:floor(lc(l)*rn_ideal(l)))]; % learning set indexes
                    data_ind = setxor([1:size(data,1)], ideal_ind); % testing set indexes
                end
                ideals(:,:,k) = idealvectors(data(ideal_ind,:), y); % idealvectors
                if w_opt == 0
                    [fitness(k), class, Simil] = calcfit(data(data_ind,:), ideals(:,:,k), y);
                end
            end
            Means(i,j,n) = mean(fitness);
            Vars(i,j,n) = var(fitness);
            Maxsf(i,j,n) = max(fitness);
            Minsf(i,j,n) = min(fitness);
            fitness=[];
        end
    end
    tmp=max(max(Means));
    [p1,m1]=find(tmp==Means);
    Mean_accuracy=Means(p1,m1);
    Variance=Vars(p1,m1);
    if pl==1
    [X,Y] = meshgrid(m,p);
    figure
    subplot(2,2,2);
    surfc(X,Y,Vars(:,:,n))
    title('Variances','FontSize',15)
    xlabel('\alpha-values')
    ylabel('p-values')
    zlabel('Variance')
    subplot(2,2,1)
    surfc(X,Y,Means(:,:,n))
    title('Mean classification accuracies','FontSize',15)
    xlabel('\alpha-values')
    ylabel('p-values')
    zlabel('Classification accuracy')
     subplot(2,2,3)
     surfc(X,Y,Maxsf(:,:,n))
     title('Maximum accuracies','FontSize',15)
     xlabel('\alpha-values')
     ylabel('p-values')
     zlabel('Classification accuracy')
     subplot(2,2,4)
     surfc(X,Y,Minsf(:,:,n))
     title('Minimum accuracies','FontSize',15)
     xlabel('\alpha-values')
     ylabel('p-values')
     zlabel('Classification accuracy')
    end
    clear Y
end
if length(sv_name) ~= 0
    save(sv_name)
end


function [dpixc, dveccr, N] = generatedata(d, sizevec, psf, maxphot, offset, Nt, rs)

nx = sizevec(2)-sizevec(1);
ny = sizevec(4)-sizevec(3);
N = size(d,1);

blinkmat = rand(N, Nt);

for ii=1 : N
    dpix_ind(:,:,ii) = pixelize(d(ii,:), 1, sizevec, nx, ny, [],0);
    dpixc_ind(:,:,ii) = conv2(dpix_ind(:,:,ii),psf,'same'); %convolution of individual points
end

dvecc_ind = squeeze(reshape(dpixc_ind, nx*ny, 1, N)); % each indiv image to vector

dvecc_nonoise = (dvecc_ind*blinkmat); % set of Nt vectors
dpixc_nonoise = imresize(reshape(dvecc_nonoise, nx, ny, Nt), rs);
dpixc_nonoise_dip = dip_image(dpixc_nonoise);

maxdvec = max(dpixc_nonoise(:));
offset_abs = offset/(1-offset)*maxdvec;
dpixc_nonoise_dip = (dpixc_nonoise_dip + offset_abs)/(maxdvec+offset_abs)*maxphot;
dpixc_dip = noise(dpixc_nonoise_dip,'poisson');
dpixc = double(dpixc_dip);
dveccr = double(squeeze(reshape(dpixc, nx*ny*rs^2, 1, Nt))); % vectors of resized images