function [w,u] = biharmonicP3(node,elem,pde,bdFlag,option)
%   [w,u] = biharmonicP1(node,elem,pde,bdFlag) produces the mixed cubic finite element
%   approximation of the biharmonic equation, where w = laplace u
%   See also biharmonicP1, biharmonicP2, biharmonicP3.
%   Created by Jie Zhou.
%   Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if nargin<5, option = []; end
tic;
%% Construct Data Structure
[elem2dof,elem2edge,edge,fixedDof,freeDof] = dofP3(elem);
N = size(node,1);  NT = size(elem,1); NE=size(edge,1); Ndof = N+2*NE+NT;
%% Compute geometric quantities and gradient of local basis
[Dlambda,area] = gradbasis(node,elem);

%% Assemble stiffness matrix
% Since Dphi_i*Dphi_j is quadratic, numerical quadrature rule is used here
if ~isfield(option,'quadorder')
    option.quadorder = 6;   % default order
end
[lambda, weight] = quadpts(option.quadorder);
nQuad = size(lambda,1);
ii = zeros(55*NT,1); jj = zeros(55*NT,1); sA = zeros(55*NT,nQuad);sB = zeros(55*NT,nQuad);
index = 0;
for i = 1:10
    for j = i:10
        ii(index+1:index+NT) = double(elem2dof(:,i));
        jj(index+1:index+NT) = double(elem2dof(:,j));
        index = index + NT;
    end
end

for p = 1:nQuad
        % Dphi at quadrature points
    Dphi(:,:,1) = (27/2*lambda(p,1)*lambda(p,1)-9*lambda(p,1)+1).*Dlambda(:,:,1);
    Dphi(:,:,2) = (27/2*lambda(p,2)*lambda(p,2)-9*lambda(p,2)+1).*Dlambda(:,:,2);
    Dphi(:,:,3) = (27/2*lambda(p,3)*lambda(p,3)-9*lambda(p,3)+1).*Dlambda(:,:,3);
    Dphi(:,:,4) = 9/2*((3*lambda(p,2)*lambda(p,2)-lambda(p,2)).*Dlambda(:,:,3)+...
                   lambda(p,3)*(6*lambda(p,2)-1).*Dlambda(:,:,2));
    Dphi(:,:,5) = 9/2*((3*lambda(p,3)*lambda(p,3)-lambda(p,3)).*Dlambda(:,:,2)+...
                   lambda(p,2)*(6*lambda(p,3)-1).*Dlambda(:,:,3));
    Dphi(:,:,6) = 9/2*((3*lambda(p,3)*lambda(p,3)-lambda(p,3)).*Dlambda(:,:,1)+...
                   lambda(p,1)*(6*lambda(p,3)-1).*Dlambda(:,:,3));
    Dphi(:,:,7) = 9/2*((3*lambda(p,1)*lambda(p,1)-lambda(p,1)).*Dlambda(:,:,3)+...
                   lambda(p,3)*(6*lambda(p,1)-1).*Dlambda(:,:,1));
    Dphi(:,:,8) = 9/2*((3*lambda(p,1)*lambda(p,1)-lambda(p,1)).*Dlambda(:,:,2)+...
                   lambda(p,2)*(6*lambda(p,1)-1).*Dlambda(:,:,1));
    Dphi(:,:,9)  = 9/2*((3*lambda(p,2)*lambda(p,2)-lambda(p,2)).*Dlambda(:,:,1)+...
                   lambda(p,1)*(6*lambda(p,2)-1).*Dlambda(:,:,2));
    Dphi(:,:,10) = 27*(lambda(p,1)*lambda(p,2)*Dlambda(:,:,3)+lambda(p,1)*lambda(p,3)*Dlambda(:,:,2)+...
                   lambda(p,3)*lambda(p,2)*Dlambda(:,:,1));
    phi(:,1) = 0.5*(3*lambda(:,1)-1).*(3*lambda(:,1)-2).*lambda(:,1);
    phi(:,2) = 0.5*(3*lambda(:,2)-1).*(3*lambda(:,2)-2).*lambda(:,2);
    phi(:,3) = 0.5*(3*lambda(:,3)-1).*(3*lambda(:,3)-2).*lambda(:,3);
    phi(:,4) = 9/2*lambda(:,3).*lambda(:,2).*(3*lambda(:,2)-1);
    phi(:,5) = 9/2*lambda(:,3).*lambda(:,2).*(3*lambda(:,3)-1);
    phi(:,6) = 9/2*lambda(:,1).*lambda(:,3).*(3*lambda(:,3)-1);
    phi(:,7) = 9/2*lambda(:,1).*lambda(:,3).*(3*lambda(:,1)-1);
    phi(:,8) = 9/2*lambda(:,1).*lambda(:,2).*(3*lambda(:,1)-1);
    phi(:,9) = 9/2*lambda(:,1).*lambda(:,2).*(3*lambda(:,2)-1);
    phi(:,10) = 27*lambda(:,1).*lambda(:,2).*lambda(:,3);
    index = 0;
    for i = 1:10
    for j = i:10
        Bij = 0;
        Aij = 0;
                  Bij = Bij + weight(p)*dot(Dphi(:,:,i),Dphi(:,:,j),2);
                  Aij = Aij + weight(p)*dot(phi(p,i),phi(p,j),2);
        Bij = Bij.*area;
        Aij = Aij.*area;
        ii(index+1:index+NT) = double(elem2dof(:,i));
        jj(index+1:index+NT) = double(elem2dof(:,j));
        sB(index+1:index+NT,p) = Bij;
        sA(index+1:index+NT,p) = Aij;
        index = index + NT;
    end
    end
end

sA = sum(sA,2);
sB = sum(sB,2);
clear Aij Bij
diagIdx = (ii == jj);   upperIdx = ~diagIdx;
B = sparse(ii(diagIdx),jj(diagIdx),sB(diagIdx),Ndof,Ndof);
A = sparse(ii(diagIdx),jj(diagIdx),sA(diagIdx),Ndof,Ndof);
% A = spdiags(accumarray(ii(diagIdx),sA(diagIdx),[Ndof 1]),0,Ndof,Ndof);
BU = sparse(ii(upperIdx),jj(upperIdx),sB(upperIdx),Ndof,Ndof);
AU = sparse(ii(upperIdx),jj(upperIdx),sA(upperIdx),Ndof,Ndof);
B = B + BU + BU';
A = A + AU + AU';

% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % subfunction Dphi
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%     function s = Dphi(p,i) % gradient of basis phi
%     switch i
%         case 1
%             s =(27/2*lambda(p,1)*lambda(p,1)-9*lambda(p,1)+1).*Dlambda(:,:,1);
%         case 2
%             s =(27/2*lambda(p,2)*lambda(p,2)-9*lambda(p,2)+1).*Dlambda(:,:,2);
%         case 3
%             s =(27/2*lambda(p,3)*lambda(p,3)-9*lambda(p,3)+1).*Dlambda(:,:,3);
%          case 4
%             s =9/2*((3*lambda(p,2)*lambda(p,2)-lambda(p,2)).*Dlambda(:,:,3)+...
%                 lambda(p,3)*(6*lambda(p,2)-1).*Dlambda(:,:,2));
%          case 5
%             s =9/2*((3*lambda(p,3)*lambda(p,3)-lambda(p,3)).*Dlambda(:,:,2)+...
%                 lambda(p,2)*(6*lambda(p,3)-1).*Dlambda(:,:,3));
%
%          case 6
%             s =9/2*((3*lambda(p,3)*lambda(p,3)-lambda(p,3)).*Dlambda(:,:,1)+...
%                 lambda(p,1)*(6*lambda(p,3)-1).*Dlambda(:,:,3));
%          case 7
%             s =9/2*((3*lambda(p,1)*lambda(p,1)-lambda(p,1)).*Dlambda(:,:,3)+...
%                 lambda(p,3)*(6*lambda(p,1)-1).*Dlambda(:,:,1));
%          case 8
%             s =9/2*((3*lambda(p,1)*lambda(p,1)-lambda(p,1)).*Dlambda(:,:,2)+...
%                 lambda(p,2)*(6*lambda(p,1)-1).*Dlambda(:,:,1));
%
%           case 9
%             s =9/2*((3*lambda(p,2)*lambda(p,2)-lambda(p,2)).*Dlambda(:,:,1)+...
%                 lambda(p,1)*(6*lambda(p,2)-1).*Dlambda(:,:,2));
%          case 10
%             s = 27*(lambda(p,1)*lambda(p,2)*Dlambda(:,:,3)+lambda(p,1)*lambda(p,3)*Dlambda(:,:,2)+...
%             lambda(p,3)*lambda(p,2)*Dlambda(:,:,1));
%
%     end
%     end
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % subfunction phi
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%     function s = phi(p,i) % gradient of basis phi
%     switch i
%         case 1
%             s = 0.5*(3*lambda(p,1)-1).*(3*lambda(p,1)-2).*lambda(p,1);
%         case 2
%             s = 0.5*(3*lambda(p,2)-1).*(3*lambda(p,2)-2).*lambda(p,2);
%         case 3
%             s = 0.5*(3*lambda(p,3)-1).*(3*lambda(p,3)-2).*lambda(p,3);
%         case 4
%             s = 9/2*lambda(p,3).*lambda(p,2).*(3*lambda(p,2)-1);
%         case 5
%             s = 9/2*lambda(p,3).*lambda(p,2).*(3*lambda(p,3)-1);
%         case 6
%             s = 9/2*lambda(p,1).*lambda(p,3).*(3*lambda(p,3)-1);
%         case 7
%             s = 9/2*lambda(p,1).*lambda(p,3).*(3*lambda(p,1)-1);
%         case 8
%             s = 9/2*lambda(p,1).*lambda(p,2).*(3*lambda(p,1)-1);
%         case 9
%             s = 9/2*lambda(p,1).*lambda(p,2).*(3*lambda(p,2)-1);
%         case 10
%             s = 27*lambda(p,1).*lambda(p,2).*lambda(p,3);
%     end
%     end
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Assemble right hand side by high order quadrature rule
% To reduce the effect of the error introduced by the numerical quadrature,
% the load term is computed using the 3rd order qudrature rule.
b = zeros(Ndof,1);
u = zeros(Ndof,1);
w = zeros(Ndof,1);

if ~isfield(option,'fquadorder')
    option.fquadorder = 6;   % default order
end
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end
if ~isempty(pde.f)
    % quadrature points in the barycentric coordinate
    [lambda,w] = quadpts(option.fquadorder);
    nQuad = size(lambda,1);
%     phi(:,1) = lambda(:,1).*(2*lambda(:,1)-1);
%     phi(:,2) = lambda(:,2).*(2*lambda(:,2)-1);
%     phi(:,3) = lambda(:,3).*(2*lambda(:,3)-1);
%     phi(:,4) = 4*lambda(:,2).*lambda(:,3);
%     phi(:,5) = 4*lambda(:,3).*lambda(:,1);
%     phi(:,6) = 4*lambda(:,1).*lambda(:,2);
    bt = zeros(NT,10);
    for p = 1:nQuad
        % quadrature points in the x-y coordinate
        pxy = lambda(p,1)*node(elem(:,1),:) ...
            + lambda(p,2)*node(elem(:,2),:) ...
            + lambda(p,3)*node(elem(:,3),:);
        if isfield(pde,'f') && isnumeric(pde.f)
            fp = pde.f;        % piecewise constant
        else
            fp = pde.f(pxy);   % function handle
        end
        for j = 1:10
            bt(:,j) = bt(:,j) + w(p)*phi(p,j)*fp;
        end
    end
    bt = bt.*repmat(area,1,10);
     b = accumarray(elem2dof(:),bt(:),[Ndof 1]);
end

[b1,u] = getbdP3(b);
% b1 is the first part of the right sides, b is the second part of right sides

 function [b1,u] = getbdP3(b)
    %% Boundary conditions for Poisson equation: P3 quadratic FEM.
    %
    % The set up of boundary condition consists of two parts:
    %

    %
    %  Modify the right hand side b. The Neumann boundary integral is added
    %  to b. For Dirichlet boundary ndoes, b(fixedDof) is the evaluation of
    % pde.g_D.
    %
    % Special attentation should be given for the pure Neumann boundary
    % condition. To enforce the compatible condition, the vector b should have
    % mean value zero. To avoid a singular matrix, the 1st node is chosen as
    % fixedDof.
    %
    % The order of assigning Neumann and Dirichlet boundary condition is
    % important to get the right setting at the intersection nodes of Dirichlet
    % and Neumann boundary edges.

    u = zeros(Ndof,1);

    %% Part 1: Find boundary edges and modify the load b
    % Neumann boundary condition
        if ~isfield(option,'gNquadorder')
            option.gNquadorder = 6;
        end
        b1 = zeros(Ndof,1);
        [lambdagN,weightgN] = quadpts1(option.gNquadorder);
                idxN = (bdFlag(:) == 1);      % all Neumann edges in bdFlag
        Neumannidx = elem2edge(idxN ); % index of Neumann and Robin edges
        % since boundary integral is also needed for Robin edges
        Neumann   = edge(Neumannidx,:);

        nQuadgN = size(lambdagN,1);
        % quadratic bases (1---3---4--2)
        bdphi = zeros(nQuadgN,4);
        bdphi(:,1) = 0.5*(3*lambdagN(:,1)-1).*(3*lambdagN(:,1)-2).*lambdagN(:,1);
        bdphi(:,2) = 0.5*(3*lambdagN(:,2)-1).*(3*lambdagN(:,2)-2).*lambdagN(:,2);
        bdphi(:,3) = 9/2*lambdagN(:,1).*lambdagN(:,2).*(3*lambdagN(:,1)-1);
        bdphi(:,4) = 9/2*lambdagN(:,1).*lambdagN(:,2).*(3*lambdagN(:,2)-1);
        % length of edge

        el = sqrt(sum((node(Neumann(:,1),:) - node(Neumann(:,2),:)).^2,2));
        ge = zeros(size(Neumann,1),4);
        for pp = 1:nQuadgN
            ppxy = lambdagN(pp,1)*node(Neumann(:,1),:) ...
                 + lambdagN(pp,2)*node(Neumann(:,2),:);
            gNu = pde.g_N(ppxy);
            ge(:,1) = ge(:,1) + weightgN(pp)*gNu*bdphi(pp,1);
            ge(:,2) = ge(:,2) + weightgN(pp)*gNu*bdphi(pp,2);
            ge(:,3) = ge(:,3) + weightgN(pp)*gNu*bdphi(pp,3);
            ge(:,4) = ge(:,4) + weightgN(pp)*gNu*bdphi(pp,4);
        end
        ge = ge.*repmat(el,1,4);
        b1(1:N) =  accumarray(Neumann(:), [ge(:,1); ge(:,2)],[N,1]);
        b1(N+2*Neumannidx-1) = b1(N+2*Neumannidx-1) + ge(:,3);
        b1(N+2*Neumannidx)   = b1(N+2*Neumannidx) + ge(:,4);

    %% Part 2: Find Dirichlet boundary edges and compute the boundary value
    % Dirichlet boundary conditions

        isDirichlet(elem2edge(bdFlag(:)==1)) = true;
        % interpolation
        idx = (fixedDof > N);         % index of edge nodes
        u(fixedDof(~idx)) = pde.g_D(node(fixedDof(~idx),:)); % bd value at vertex dofs
        % for P3,  we should divide the points of edge into two parts.
        bdEdgeIdx = fixedDof(idx) - N;
        %  First parts, the points  * is in  1---*------2
        bdEdgeMid = node(edge(isDirichlet,1),:)+(node(edge(isDirichlet,2),:) ...
                  - node(edge(isDirichlet,1),:))/3;
        u(N + bdEdgeIdx(1:2:end)) = pde.g_D(bdEdgeMid);
      %  Second parts, the points * is in  1------*---2
        bdEdgeMid = node(edge(isDirichlet,1),:)+2*(node(edge(isDirichlet,2),:)...
                  - node(edge(isDirichlet,1),:))/3;
        u(N + bdEdgeIdx(2:2:end)) = pde.g_D(bdEdgeMid);
        % modify the right hand side
         b1 = b1 - B*u;
         %b1(fixedDof) = u(fixedDof);

    end % end of getbdP3

B(:,fixedDof)=[];
Nu = Ndof-size(fixedDof,1);

b(fixedDof) = [];
     bigA = [A, B; ...
             B', sparse(Nu,Nu)];
     bigF = [b1; -b];
% Solver
%bigU=PFGMRES(bigA, bigF,sparse(Ndof+Nu,1), Ndof, 10, 1e-6, [],[]);
tic;
bigU=bigA\bigF;
toc;
%   norm(bigA*bigU-bigF)
%   norm(bigA*[p;u(freeDof)]-bigF)
   w=bigU(1:Ndof);
   u(freeDof)=bigU(Ndof+1:Ndof+Nu);

end                 % end of function PoissonP2


function q = ch_qnormlz(q)
% 四元数归一化
    q = q/norm(q);
    if(q(1)<0)
        q(1) = -q(1);
        q(2) = -q(2);
        q(3) = -q(3);
        q(4) = -q(4);
    end


% SYNTAX:
% dc = hmrR_OD2Conc( dod, probe, ppf )
%
% UI NAME:
% OD_to_Conc
%
% DESCRIPTION:
% Convert OD to concentrations.
%
% INPUTS:
% dod: SNIRF.data container with the Change in OD tim course
% probe: SNIRF.probe container with the source/detector geometry
% ppf: Partial path length factors for each wavelength. This is a vector of
%      factors per wavelength.  Typical value is ~6 for each
%      wavelength if the absorption change is uniform over the volume of tissue measured.
%      To approximate the partial volume effect of a small localized absorption change
%      within an adult human head, this value could be as small as 0.1. Convention is
%      becoming to set ppf=1 and to not divide by the source-detector separation such that
%      the resultant "concentration" is in units of Molar mm (or Molar cm if those are the
%      spatial units). This is becoming wide spread in the literature but there is no
%      fixed citation. Use a value of 1 to choose this option.
%
% OUTPUTS:
% dc: SNIRF.data container with the concentration data
%
% USAGE OPTIONS:
% Delta_OD_to_Conc: dc = hmrR_OD2Conc( dod, probe, ppf )
%
% PARAMETERS:
% ppf: [1.0, 1.0]
%
% PREREQUISITES:
% Intensity_to_Delta_OD: dod = hmrR_Intensity2OD( intensity )
function dc = hmrR_OD2Conc( dod, probe, ppf )

dc = DataClass().empty();

for ii=1:length(dod)
    dc(ii) = DataClass();

    Lambda = probe.GetWls();
    SrcPos = probe.GetSrcPos();
    DetPos = probe.GetDetPos();
    nWav   = length(Lambda);
    ml     = dod(ii).GetMeasList();
    y      = dod(ii).GetDataTimeSeries();

    if length(ppf) < nWav
        warning('Length of ppf does not match the number of wavelengths. Falling back to ppf=1 for all wavelengths.');
        ppf = ones(1, nWav);
    elseif length(ppf) > nWav
        d = length(ppf)-nWav;
        ppf(end-d+1:end) = [];
    end

    if ~isempty(find(ppf==1))
        ppf = ones(size(ppf));
    end

    nTpts = size(y,1);

    e = GetExtinctions(Lambda);
    e = e(:,1:2) / 10; % convert from /cm to /mm
    einv = inv( e'*e )*e';

    lst = find( ml(:,4)==1 );
    y2 = zeros(nTpts, 3*length(lst));
    for idx=1:length(lst)
        k = 3*(idx-1)+1;
        idx1 = lst(idx);
        idx2 = find( ml(:,4)>1 & ml(:,1)==ml(idx1,1) & ml(:,2)==ml(idx1,2) );
        rho = norm(SrcPos(ml(idx1,1),:)-DetPos(ml(idx1,2),:));
        if ppf(1)~=1
            y2(:,k:k+1) = ( einv * (y(:,[idx1 idx2'])./(ones(nTpts,1)*rho*ppf))' )';
        else
            y2(:,k:k+1) = ( einv * (y(:,[idx1 idx2'])./(ones(nTpts,1)))' )';
        end
        y2(:,k+2) = y2(:,k) + y2(:,k+1);
        dc(ii).AddChannelHbO(ml(idx1,1), ml(idx1,2));
        dc(ii).AddChannelHbR(ml(idx1,1), ml(idx1,2));
        dc(ii).AddChannelHbT(ml(idx1,1), ml(idx1,2));
    end
    dc(ii).SetDataTimeSeries(y2);
    dc(ii).SetTime(dod(ii).GetTime());
end


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

%%% H-MATRIX BOUNDARY ELEMENT OPERATOR  --> \int_{mesh(x)} \int_{mesh(y)} psi(x)' f(x,y) psi(y) dxdy
if isa(data{1},'dom') && isa(data{2},'dom')
    % Domain with quadrature
    Xdom   = data{1};
    [X,Wx] = Xdom.qud;
    Nx     = size(X,1);
    Wx     = spdiags(Wx,0,Nx,Nx);

    % Domain with quadrature
    Ydom   = data{2};
    [Y,Wy] = Ydom.qud;
    Ny     = size(Y,1);
    Wy     = spdiags(Wy,0,Ny,Ny);

    % Finite element matrix with integration
    u  = data{3};
    Mu = u.uqm(Xdom);
    if iscell(Mu)
        Mu{1} = Mu{1}' * Wx;
        Mu{2} = Mu{2}' * Wx;
        Mu{3} = Mu{3}' * Wx;
    else
        Mu = Mu' * Wx;
    end

    % Green kernel
    green = data{4};

    % Finite element matrix with integration
    v  = data{5};
    Mv = v.uqm(Ydom);
    if iscell(Mv)
        Mv{1} = Wy * Mv{1};
        Mv{2} = Wy * Mv{2};
        Mv{3} = Wy * Mv{3};
    else
        Mv = Wy * Mv;
    end

    % Accuracy
    tol = data{6};

    % H-Matrix Integration
    if iscell(Mu) && ~iscell(green) && ~iscell(Mv)
        I{1} = hmx(u.unk,v.unk,Mu{1},X,green,Y,Mv,tol);
        I{2} = hmx(u.unk,v.unk,Mu{2},X,green,Y,Mv,tol);
        I{3} = hmx(u.unk,v.unk,Mu{3},X,green,Y,Mv,tol);

    elseif ~iscell(Mu) && iscell(green) && ~iscell(Mv)
        I{1} = hmx(u.unk,v.unk,Mu,X,green{1},Y,Mv,tol);
        I{2} = hmx(u.unk,v.unk,Mu,X,green{2},Y,Mv,tol);
        I{3} = hmx(u.unk,v.unk,Mu,X,green{3},Y,Mv,tol);

    elseif ~iscell(Mu) && ~iscell(green) && iscell(Mv)
        I{1} = hmx(u.unk,v.unk,Mu,X,green,Y,Mv{1},tol);
        I{2} = hmx(u.unk,v.unk,Mu,X,green,Y,Mv{2},tol);
        I{3} = hmx(u.unk,v.unk,Mu,X,green,Y,Mv{3},tol);

    else
        I = hmx(u.unk,v.unk,Mu,X,green,Y,Mv,tol);
    end

%%% FFM BOUNDARY ELEMENT INTEGRATION --> \int_{mesh(y)} f(x,y) psi(y) dy
elseif isnumeric(data{1}) && isa(data{2},'dom')
    % Evaluation points
    X  = data{1};
    Nx = size(X,1);
    Mx = speye(Nx,Nx);

    % Domain with quadrature
    Ydom   = data{2};
    [Y,Wy] = Ydom.qud;
    Ny     = size(Y,1);
    Wy     = spdiags(Wy,0,Ny,Ny);

    % Green kernel
    green = data{3};

    % Wave number
    k = data{4};

    % Integrated finite element matrix
    v  = data{5};
    Mv = v.uqm(Ydom);
    if iscell(Mv)
        Mv{1} = Wy * Mv{1};
        Mv{2} = Wy * Mv{2};
        Mv{3} = Wy * Mv{3};
    else
        Mv = Wy * Mv;
    end

    % Accuracy
    tol = data{6};

    % Fast & Furious integration
    if iscell(Mv) && iscell(green)
        I = ffm(Mx,X,green{1},k,Y,Mv{1},tol) + ...
            ffm(Mx,X,green{2},k,Y,Mv{2},tol) + ...
            ffm(Mx,X,green{3},k,Y,Mv{3},tol);

    elseif iscell(Mv) && ~iscell(green)
        I{1} = ffm(Mx,X,green,k,Y,Mv{1},tol);
        I{2} = ffm(Mx,X,green,k,Y,Mv{2},tol);
        I{3} = ffm(Mx,X,green,k,Y,Mv{3},tol);

    elseif ~iscell(Mv) && iscell(green)
        I{1} = ffm(Mx,X,green{1},k,Y,Mv,tol);
        I{2} = ffm(Mx,X,green{2},k,Y,Mv,tol);
        I{3} = ffm(Mx,X,green{3},k,Y,Mv,tol);

    else
        I = ffm(Mx,X,green,k,Y,Mv,tol);
    end

%%% FFM BOUNDARY ELEMENT INTEGRATION --> \int_{mesh(x)} psi(x)' f(x,y) dx
elseif isa(data{1},'dom') && isnumeric(data{2})
    % Domain with quadrature
    Xdom   = data{1};
    [X,Wx] = Xdom.qud;
    Nx     = size(X,1);
    Wx     = spdiags(Wx,0,Nx,Nx);

    % Evaluation points
    Y  = data{2};
    Ny = size(Y,1);
    My = speye(Ny,Ny);

    % Integrated finite element matrix
    u  = data{3};
    Mu = u.uqm(Xdom);
    if iscell(Mu)
        Mu{1} = Mu{1}' * Wx;
        Mu{2} = Mu{2}' * Wx;
        Mu{3} = Mu{3}' * Wx;
    else
        Mu = Mu' * Wx;
    end

    % Green kernel
    green = data{4};

    % Wave number
    k = data{5};

    % Accuracy
    tol = data{6};

    % Fast & Furious integration
    if iscell(Mu) && iscell(green)
        I = ffm(Mu{1},X,green{1},k,Y,My,tol) + ...
            ffm(Mu{2},X,green{2},k,Y,My,tol) + ...
            ffm(Mu{3},X,green{3},k,Y,My,tol) ;

    elseif iscell(Mu) && ~iscell(green)
        I{1} = ffm(Mu{1},X,green,k,Y,My,tol);
        I{2} = ffm(Mu{2},X,green,k,Y,My,tol);
        I{3} = ffm(Mu{3},X,green,k,Y,My,tol);

    elseif ~iscell(Mu) && iscell(green)
        I{1} = ffm(Mu,X,green{1},k,Y,My,tol);
        I{2} = ffm(Mu,X,green{2},k,Y,My,tol);
        I{3} = ffm(Mu,X,green{3},k,Y,My,tol);

    else
        I = ffm(Mu,X,green,k,Y,My,tol);
    end

else
    error('domIntegral6.m : unavailable case')
end
end


ezplot('tan(x)')


clear; clc;
tic;
% [dn,fs1] = audioread('C:\Users\Xu\Desktop\��׿��\echo cancellation\subband\book\After reading Chapter 4 -- Subband Adaptive Filtering\room1_2m_pb\001_2mplayback_1646-802.wav');
% [un,fs1] = audioread('x45.wav');
[x,fs1] = audioread('C:\Users\Xu\Desktop\��׿��\echo cancellation\subband\book\After reading Chapter 4 -- Subband Adaptive Filtering\room1_2m_pb\015_2mplayback_1646-802.wav');
dn = x(:,4);
un = x(:,5);
% Arguments:
% w0               Coefficients of FIR filter at start
% mu               Step size
% N                Number of subbands
% L                length of analysis filter
% M                length of adaptive weight vector
% H                Analysis filter bank (optional), each column represents a filter
% F                Synthesis filter bank (optional), each column represents a filter
% alpha            Adjust scaling of tap weights
% delta            Small constant
M = 512;
w0 = zeros(M,1);
mu = 0.5;
N = 8;
L = 512;
alpha = 1;
delta = 0.0001;
S = PMSAFinit(w0,mu,N,L,alpha,delta)
[en,S] = PMSAFadapt(un,dn,S);
% % result = un'-1*en;
% % plot(dn'); hold on; plot(1*en);
% % legend('dn','1 en')
toc;
soundsc(en,fs1);
pause;
clear sound
% audiowrite('4.wav',en,fs1);

github-repos/MATLAB/CharlesThaCat-acoustic-interference-cancellation/acoustic-interference-cancellation-edb394499ea6f9c96445a3e9613bd64a854c289e/Subband processing/Book/After reading Chapter 5 -- Critically Sampled and Oversampled Subband Structures/AIC.m


function [min] = us2min(us)
% Convert time from microseconds to minutes.
% Chad Greene 2012
min = us*1.666666666667e-8;


function W = lssvc(A, TYPE, PAR, C)
%LSSVC Least-Squares Support Vector Classifier
%
%       W = lssvc(A,TYPE,PAR,C);
%
% INPUT
%   A       dataset
%   TYPE    Type of the kernel (optional; default: 'p')
%   PAR     Kernel parameter (optional; default: 1)
%   C       Regularization parameter (optional; default: 1)
%
% OUTPUT
%   W       Mapping: Least-Squares Support Vector Classifier
%
% DESCRIPTION
% Optimizes a least-squares support vector classifier for the dataset A by
% quadratic programming. The classifier can be of one of the types
% as defined by PROXM. Default is linear (TYPE = 'p', PAR = 1). The
% regularization parameter C allows for seeting the level of overfitting.
% A smaller value for C allows for more class overlap. Default C = 10.
%
% NOTE
% This implementation uses the LS-SVMLab toolbox. This toolbox is available
% from http://www.esat.kuleuven.ac.be/sista/lssvmlab/. Make sure the path
% to LS-SVMLab is set in Matlab.
%
% See also MAPPINGS, DATASETS, PROXM

% Copyright: L.J.P. van der Maaten, l.vandermaaten@micc.unimaas.nl
% MICC-IKAT, Maastricht University, Maastricht, The Netherlands

    name = 'LSSVC';

    % Check whether LS-SVMLab is installed and set in the path
    if ~exist('trainlssvm.m', 'file')
        error('Could not find LS-SVMLab. Make sure LS-SVMLab is installed and set in the Matlab path.');
    end

    % Perform LS-SVM training on dataset A
    if nargin <= 1 || (nargin >= 2 && ~isa(TYPE, 'prmapping'))

        % Set LS-SVM parameters
        if ~exist('TYPE', 'var') || ~isstruct(TYPE)
            if nargin < 2 || isempty(TYPE), data.kernel = 'p';    else data.kernel = TYPE; end       % kernel function
            if nargin < 3 || isempty(PAR),  data.par = 1;         else data.par = PAR; end           % kernel parameters
            if nargin < 4 || isempty(C),    data.gamma = 10;      else data.gamma = C; end           % regularization parameter
            data.par2 = -1;

            % LS-SVMLab uses different naming for kernels; correct this
            switch lower(data.kernel)
                case {'p', 'polynomial'},       data.kernel = 'poly_kernel'; data.par2 = 1;
                case {'h', 'homogeneous'},      data.kernel = 'poly_kernel'; data.par2 = 0;
                case {'e', 'exponential'},      error('This kernel is not supported by LS-SVMLab.');
                case {'r', 'radial_basis'},     data.kernel = 'RBF_kernel'; data.par = data.par^2;          % LS-SVMLab uses sigma^2 as input
                case {'s', 'sigmoid'},          data.kernel = 'MLP_kernel';
                case {'d', 'distance'},         error('This kernel is not supported by LS-SVMLab.');
                case {'m', 'minkowski'},        error('This kernel is not supported by LS-SVMLab.');
                case {'c', 'city-block'},       error('This kernel is not supported by LS-SVMLab.');
                case {'o', 'cosine'},           error('This kernel is not supported by LS-SVMLab.');
                otherwise, error('Unknown kernel function.');
            end
        else
            data = TYPE;
        end

        % Handle the case in which the dataset is empty
        if ~exist('A', 'var') || isempty(A)
            W = prmapping('lssvc', 'untrained', data);
            W = setname(W, name);
            return;
        end

        % Perform some checks on the data
        islabtype(A, 'crisp');          % allow crisp labels only
        isvaldset(A, 1, 2);             % at least one object per class, two objects
        if ischar(A.lablist), error('Only numerical labels are allowed in LSSVC.'); end

        % Train binary LS-SVM
        [m, k, c] = getsize(A);         % size of the training set
		if length(A.lablist) <= 2
            if strcmp(data.kernel, 'poly_kernel')
                [alpha, b] = trainlssvm({A.data, A.labels, 'classification', data.gamma, [data.par2; data.par], data.kernel});
            else
                [alpha, b] = trainlssvm({A.data, A.labels, 'classification', data.gamma, data.par, data.kernel});
            end

		% Train multi-class LS-SVM
		else
			[labels_code, codebook, old_codebook] = code(A.labels, 'code_MOC');
            if strcmp(data.kernel, 'poly_kernel')
                [alpha, b] = trainlssvm({A.data, labels_code, 'classification', data.gamma, [data.par2; data.par], data.kernel});
            else
                [alpha, b] = trainlssvm({A.data, labels_code, 'classification', data.gamma, data.par, data.kernel});
            end
		end

        % Store results of training in data-struct
        data.A = A;
        data.alpha = alpha;
        data.b = b;
		if length(A.lablist) > 2
			data.labels_code = labels_code;
			data.codebook = codebook;
			data.old_codebook = old_codebook;
		end
        W = prmapping('lssvc', 'trained', data, getlablist(A), k, c);
        W = setname(W, name);

    % Apply LS-SVM mapping B on dataset A
    elseif nargin == 2 && isa(TYPE, 'prmapping')

        % Initialize some variables
        B = TYPE;
        [m, k] = getsize(A);        % size of the test set
        [k, c] = size(B);           % K features with C classes
        W = zeros(m, c);            % output: C class densities for M objects

        % Perform binary classification
		if length(B.data.A.lablist) <= 2
            if strcmp(B.data.kernel, 'poly_kernel')
                C = simlssvm({B.data.A.data, B.data.A.labels, 'classification', B.data.gamma, [B.data.par2; B.data.par], B.data.kernel, 'preprocess'}, ...
                             {B.data.alpha, B.data.b}, A.data);
            else
				C = simlssvm({B.data.A.data, B.data.A.labels, 'classification', B.data.gamma, B.data.par, B.data.kernel, 'preprocess'}, ...
                             {B.data.alpha, B.data.b}, A.data);
            end

		% Perform multi-class classification (only numerical labels!)
        else
            if strcmp(B.data.kernel, 'poly_kernel')
                C = simlssvm({B.data.A.data, B.data.labels_code, 'classification', B.data.gamma, [B.data.par2; B.data.par], B.data.kernel, 'preprocess'}, ...
                             {B.data.alpha, B.data.b}, A.data);
            else
                C = simlssvm({B.data.A.data, B.data.labels_code, 'classification', B.data.gamma, B.data.par, B.data.kernel, 'preprocess'}, ...
                             {B.data.alpha, B.data.b}, A.data);
            end
			C = code(C, B.data.old_codebook, [], B.data.codebook, 'codedist_hamming');
        end

        % Make sure all labels are numbered in the same way as in A
        for i=1:length(C)
            [tf, loc] = ismember(B.data.A.lablist, C(i));
            W(i, find(loc)) = 1;
        end

        % Return new labeled dataset
        W = setdat(A, W, B);

    % Should not happen
    else
        error('Illegal call.');
    end


function [u v] = coarse_to_fine(I1, I2, settings, show_flow, h)

[M N] = size(I1);

% computes the maximum number of pyramid levels; the coarsest image should
% have a width or height around 10
pyramid_levels = min(...
    ceil(log(10/M)/log(settings.pyramid_factor)), ...
    ceil(log(10/N)/log(settings.pyramid_factor)));

pyrI1 = cell(pyramid_levels, 1);
pyrI2 = cell(pyramid_levels, 1);

pyrI1{1} = I1;
pyrI2{1} = I2;

% build the pyramids
for i = 2:pyramid_levels
  pyrI1{i} = imresize(I1, (settings.pyramid_factor)^(i-1), settings.resampling_method);
  pyrI2{i} = imresize(I2, (settings.pyramid_factor)^(i-1), settings.resampling_method);
end

% start coarse to fine processing
for level = pyramid_levels:-1:1;

  [M N] = size(pyrI1{level});
  if level == pyramid_levels

    % initialization
    u = zeros(M, N);
    v = zeros(M, N);

    pu = zeros(M, N, 2);
    pv = zeros(M, N, 2);

  else
    % previous dimensions
    [Mp Np] = size(pyrI1{level+1});

    % upsample the flow to next level
    u = imresize(u, [M N], settings.resampling_method) * N/Np;
    v = imresize(v, [M N], settings.resampling_method) * M/Mp;

    pu_tmp = pu;
    pv_tmp = pv;

    pu = zeros(M, N, 2);
    pv = zeros(M, N, 2);

    for i=1:2
      pu(:,:,i) = imresize(pu_tmp(:,:,i), [M N], settings.resampling_method);
      pv(:,:,i) = imresize(pv_tmp(:,:,i), [M N], settings.resampling_method);
    end
  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % solve the optical flow on the current level
  [u, v, pu, pv] = solve_flow_on_level(u, v, pu, pv, pyrI1{level}, pyrI2{level}, settings, level, show_flow, h);
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

end


function [update] = ...
    polynomial_expansion_registration3d(moving, fixed, varargin)
% POLYNOMIAL_EXPANSION_REGISTRATION3D Estimates a displacement field using polynomial expansion
%
% INPUT ARGUMENTS
% moving            - Moving image
% fixed             - Fixed image
%
% OPTIONAL INPUT ARGUMENTS
% 'signalModel'             - Local signal model to use when computing the
%                             polynomial expansion transformation
%                             'linear' (deafult), 'quadratic'
%
% 'transformationModel'     - Transformation model for estimating the
%                             displacement field
%                             translation, affine, non-rigid (default)
%
% 'multiModal'              - Set wheteher to perform multi-modal or
%                             uni-modal image registration
%                             false (default), true
%
% 'numberOfChannels'        - Number of channels to use in when computing
%                             the entropy (based on channel coding). This
%                             is only relevant if multiModal is set to
%                             true.
%                             Default value is 8
%
% OUTPUT ARGUMENTS
% update
%   displacementUpdate      - Estimated update field
%   certaintyUpdate         - Certainty related to the estimated update field
%   transformationMatrix    - Estimate transformation matrix (only if
%                             transformation model is set to translation or affine)

% Copyright (c) 2012 Daniel Forsberg
% danne.forsberg@outlook.com
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 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/>.

%% Setup default parameters
% linear, quadratic
signalModel = 'linear';

% translation, affine, non-rigid
transformationModel = 'non-rigid';
multiModal = false;

% Only valid for multi-modal registration
numberOfChannels = 16;

% Only valid for non-rigid registration
sigma = 1.5;
alpha = 0.01;

% Overwrites default parameter
for k=1:2:length(varargin)
    eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

%% Perform polynomial expansion
if strcmp(signalModel,'quadratic')
    [A_moving, b_moving, c_moving] = make_Abc_fast(moving);
    [A_fixed, b_fixed, c_fixed] = make_Abc_fast(fixed);

    A = (A_moving + A_fixed)/2;
    delta_b = b_fixed - b_moving;
else
    [b_moving, c_moving] = make_bc_fast(moving);
    [b_fixed, c_fixed] = make_bc_fast(fixed);
    if multiModal
        b = b_fixed;

        [delta_c mask] = estimate_delta_c(c_fixed,c_moving,numberOfChannels);
        delta_c(mask ~= 1) = 0;
        mask = repmat(mask,[1 1 1 3]);
        b(mask ~= 1) = 0;
    else
        b = (b_moving + b_fixed)/2;

        delta_c = c_fixed - c_moving;
    end
end

if strcmp(signalModel,'quadratic')
    switch transformationModel
        case 'translation'
            [G, h] = build_G_h_quadratic3d(A, delta_b, transformationModel);
            d = G \ h;
            update.transformationMatrix = eye(4);
            update.transformationMatrix(1:3,4) = d;
        case {'rigid','affine'}
            [G, h] = build_G_h_quadratic3d(A, delta_b, transformationModel);
            p = G \ h;

            update.transformationMatrix = eye(4);
            update.transformationMatrix(1:3,1:4) = [1+p(1) p(2) p(3) p(10);...
                p(4) 1+p(5) p(6) p(11);...
                p(7) p(8) 1+p(9) p(12)];
        case 'non-rigid'
            A11 = A(:,:,:,5);
            A12 = 2*A(:,:,:,4);
            A13 = 2*A(:,:,:,8);
            A21 = 2*A(:,:,:,2);
            A22 = A(:,:,:,1);
            A23 = 2*A(:,:,:,7);
            A31 = 2*A(:,:,:,6);
            A32 = 2*A(:,:,:,3);
            A33 = A(:,:,:,9);
            B1 = delta_b(:,:,:,2);
            B2 = delta_b(:,:,:,1);
            B3 = delta_b(:,:,:,3);

            % Set the elements of the equation system
            G11 = A11.^2 + A21.^2 + A31.^2;
            G12 = A11.*A12 + A21.*A22 + A31.*A32;
            G13 = A11.*A13 + A21.*A23 + A31.*A33;
            G22 = A12.^2 + A22.^2 + A32.^2;
            G23 = A12.*A13 + A22.*A23 + A32.*A33;
            G33 = A13.^2 + A23.^2 + A33.^2;
            H1 = A11.*B1 + A21.*B2 + A31.*B3;
            H2 = A12.*B1 + A22.*B2 + A32.*B3;
            H3 = A13.*B1 + A23.*B2 + A33.*B3;

            % Smooth the elements of the equation system
            G11 = gauss_smoothing(G11, sigma);
            G12 = gauss_smoothing(G12, sigma);
            G13 = gauss_smoothing(G13, sigma);
            G22 = gauss_smoothing(G22, sigma);
            G23 = gauss_smoothing(G23, sigma);
            G33 = gauss_smoothing(G33, sigma);
            H1 = gauss_smoothing(H1, sigma);
            H2 = gauss_smoothing(H2, sigma);
            H3 = gauss_smoothing(H3, sigma);

            % Add alpha*identity matrix for stability
            scaleFactor = max([G11(:); G22(:); G33(:)]);
            G11 = G11 + alpha * scaleFactor;
            G22 = G22 + alpha * scaleFactor;
            G33 = G33 + alpha * scaleFactor;

            det = G33.*G12.^2 - 2*G12.*G13.*G23 + G22.*G13.^2 + G11.*G23.^2 - G11.*G22.*G33;

            % Estimate the displacement field
            update.displacement = cell(3,1);
            update.displacement{1} = removenan((H1.*(G23.^2 - G22.*G33) - ...
                H3.*(G12.*G23 - G13.*G22) - ...
                H2.*(G13.*G23 - G12.*G33))./(det + eps));
            update.displacement{2} = removenan((H2.*(G13.^2 - G11.*G33) - ...
                H3.*(G12.*G13 - G11.*G23) - ...
                H1.*(G13.*G23 - G12.*G33))./(det + eps));
            update.displacement{3} = removenan((H3.*(G12.^2 - G11.*G22) - ...
                H2.*(G12.*G13 - G11.*G23) - ...
                H1.*(G12.*G23 - G13.*G22))./(det + eps));

            % Estimate a certainty
            update.certainty = sqrt(b_fixed(:,:,:,2).^2 + b_fixed(:,:,:,1).^2 + b_fixed(:,:,:,3).^2);
    end
else
    switch transformationModel
        case 'translation'
            [G, h] = build_G_h_linear3d(b, delta_c, transformationModel);
            d = G \ h;
            update.transformationMatrix = eye(4);
            update.transformationMatrix(1:3,4) = d;
        case {'rigid','affine'}
            [G, h] = build_G_h_linear3d(b, delta_c, transformationModel);
            p = G \ h;

            update.transformationMatrix = eye(4);
            update.transformationMatrix(1:3,1:4) = [1+p(1) p(2) p(3) p(10);...
                p(4) 1+p(5) p(6) p(11);...
                p(7) p(8) 1+p(9) p(12)];
        case 'non-rigid'
            % Set the elements of the equation system
            % Set the elements of the equation system
            G11 = b(:,:,:,2) .* b(:,:,:,2);
            G12 = b(:,:,:,2) .* b(:,:,:,1);
            G13 = b(:,:,:,2) .* b(:,:,:,3);
            G22 = b(:,:,:,1) .* b(:,:,:,1);
            G23 = b(:,:,:,1) .* b(:,:,:,3);
            G33 = b(:,:,:,3) .* b(:,:,:,3);
            H1 = b(:,:,:,2) .* delta_c;
            H2 = b(:,:,:,1) .* delta_c;
            H3 = b(:,:,:,3) .* delta_c;

            % Smooth the elements of the equation system
            G11 = gauss_smoothing(G11, sigma);
            G12 = gauss_smoothing(G12, sigma);
            G13 = gauss_smoothing(G13, sigma);
            G22 = gauss_smoothing(G22, sigma);
            G23 = gauss_smoothing(G23, sigma);
            G33 = gauss_smoothing(G33, sigma);
            H1 = gauss_smoothing(H1, sigma);
            H2 = gauss_smoothing(H2, sigma);
            H3 = gauss_smoothing(H3, sigma);

            % Add alpha*identity matrix for stability
            scaleFactor = max([G11(:); G22(:); G33(:)]);
            G11 = G11 + alpha * scaleFactor;
            G22 = G22 + alpha * scaleFactor;
            G33 = G33 + alpha * scaleFactor;

            det = G33.*G12.^2 - 2*G12.*G13.*G23 + G22.*G13.^2 + G11.*G23.^2 - G11.*G22.*G33;

            % Estimate the displacement field
            update.displacement = cell(3,1);
            update.displacement{1} = removenan((H1.*(G23.^2 - G22.*G33) - ...
                H3.*(G12.*G23 - G13.*G22) - ...
                H2.*(G13.*G23 - G12.*G33))./(det + eps));
            update.displacement{2} = removenan((H2.*(G13.^2 - G11.*G33) - ...
                H3.*(G12.*G13 - G11.*G23) - ...
                H1.*(G13.*G23 - G12.*G33))./(det + eps));
            update.displacement{3} = removenan((H3.*(G12.^2 - G11.*G22) - ...
                H2.*(G12.*G13 - G11.*G23) - ...
                H1.*(G12.*G23 - G13.*G22))./(det + eps));

            % Estimate a certainty
            update.certainty = sqrt(b(:,:,:,2).^2 + b(:,:,:,1).^2 + b(:,:,:,3).^2);
    end
end


% Digital Video Stabilization and Rolling Shutter Correction using Gyroscopes
% Copyright (C) 2011 Alexandre Karpenko
%
% 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
% 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 [dth, dths] = frame_rotations(gyro, gyro_time, frame_time, t0, ts)

dgt = diff(gyro_time);
theta = ((gyro(1:end-1,:) + gyro(2:end,:)) / 2) .* dgt(:,[1 1 1]);
theta = [0 0 0; cumsum(theta, 1)];
dths = lininterp(gyro_time + t0 + ts, theta, frame_time) - lininterp(gyro(:,4) + t0, theta, frame_time);

dth = diff(lininterp(gyro_time + t0, theta, frame_time));
%dth = diff(lininterp(gyro_time + t0 + ts/2, theta, frame_time));


function [param] = tps_compute_param(PP,kernel,U,Pm,Q1,Q2,R,lambda,target)
%%=====================================================================
%% $RCSfile: tps_compute_param.m,v $
%% $Author: bjian $
%% $Date: 2008/11/24 08:59:01 $
%% $Revision: 1.1 $
%%=====================================================================

TB = U*PP;
QQ = Q2*Q2';
A = inv(TB'*QQ*TB + lambda*kernel)*TB'*QQ;
tps = A*target;
affine = inv(R)*Q1'*(target-TB*tps);
param = [affine; tps];


function [parameters,ll,Ht,VCV,scores] = gogarch(data,p,q,gjrType,type,startingVals,options)
% OGARCH(p,q) and GOGARCH(p,q) multivarate volatility model estimation
%
% USAGE:
%  [PARAMETERS] = rarch(DATA,P,Q)
%  [PARAMETERS,LL,HT,VCV,SCORES] = gogarch(DATA,P,Q,GJRTYPE,TYPE,STARTINGVALS,OPTIONS)
%
% INPUTS:
%   DATA         - A T by K matrix of zero mean residuals -OR-
%                    K by K by T array of covariance estimators (e.g. realized covariance)
%   P            - Positive, scalar integer representing the number of symmetric innovations. Can
%                    also be a K by 1 vector with lag lengths for each series.
%   Q            - Non-negative, scalar integer representing the number of conditional covariance
%                    lags Can also be a K by 1 vector with lag lengths for each series.
 %  GJRTYPE      - [OPTIONAL] Either 1 (TARCH/AVGARCH) or 2 (GJR-GARCH/GARCH/ARCH).  Can also be a K
%                    by 1 vector containing the model type for each for each series. Default is 2.
%   TYPE         - [OPTIONAL] String, one of 'GOGARCH' (Default) or 'OGARCH'
%   STARTINGVALS - [OPTIONAL] Vector of starting values to use.  See parameters and COMMENTS.
%   OPTIONS      - [OPTIONAL] Options to use in the model optimization (fmincon)
%
% OUTPUTS:
%   PARAMETERS   - Estimated parameters in the order:
%                    OGARCH:
%                    [vol(1) ... vol(K)]
%                    GOGARCH:
%                    [phi(1) ... phi(K(K-1)/2) vol(1) ... vol(K)]
%                    where vol(i) = [alpha(i,1) ... alpha(i,P(i)) beta(i,1) ... beta(i,Q(i))]
%   LL           - The log likelihood at the optimum
%   HT           - A [K K T] dimension matrix of conditional covariances
%   VCV          - A numParams^2 square matrix of robust parameter covariances (A^(-1)*B*A^(-1)/T)
%   SCORES       - A T by numParams matrix of individual scores
%
% COMMENTS:
%   The orthonormal matrix is constructed from K(K-1)/2 angles using PHI2U
%
%
% EXAMPLES:
%   % OGARCH(1,1)
%   parameters = gogarch(data,1,1,[],'OGARCH')
%   % GOGARCH(1,1)
%   parameters = gogarch(data,1,1)
%
% See also BEKK, RARCH, DCC, TARCH, PHI2U

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 4/15/2012

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Argument Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
switch nargin
    case 3
        gjrType = [];
        type = [];
        startingVals = [];
        options = [];
    case 4
        type = [];
        startingVals = [];
        options = [];
    case 5
        startingVals = [];
        options = [];
    case 6
        options = [];
    case 7
        % Nothing
    otherwise
        error('3 to 7 inputs required.')
end

if ndims(data)==3
    [k,~,T] = size(data);
else
    [T,k] = size(data);
    temp = zeros(k,k,T);
    for t=1:T
        temp(:,:,t) = data(t,:)'*data(t,:);
    end
    data = temp;
end

if isscalar(p)
    p = ones(k,1)*p;
end
if isscalar(q)
    q = ones(k,1)*q;
end

if isempty(gjrType)
    gjrType = 2;
end
if isscalar(gjrType)
    gjrType = ones(k,1)*gjrType;
end

if isempty(type)
    type = 'gogarch';
end
type = lower(type);
if ~ismember(type,{'gogarch','ogarch'})
    error('TYPE must be either ''GoGARCH'' or ''OGARCH''');
end
if strcmpi(type,'gogarch')
    isGogarch = true;
else
    isGogarch = false;
end

if ~isempty(startingVals)
    count = sum(p) + sum(q);
    if isGogarch
        count = count + k*(k-1)/2;
    end
    if length(startingVals)~=count
        error('STARTINGVALS does not have the correct number of parameters.')
    end
    if isGogarch
        if any(startingVals(1:k*(k-1)/2))>pi || any(startingVals(1:k*(k-1)/2))<0
            error('STARTGINVALS 1 to K*(K-1)/2 must be between 0 and 3.141592')
        end
    end
end

if isempty(options)
    options = optimset('fmincon');
    options.Display = 'iter';
    options.Diagnostics = 'on';
    options.Algorithm = 'sqp';
else
    try
        optimset(options);
    catch ME
        error('OPTIONS does not appear to be a valid options structure.')
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Preliminary Estimation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
S = mean(data,3);
[P,L] = eig(S);
P = P';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Starting Values
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if isempty(startingVals)
    startingOptions  =  optimset('fminunc');
    startingOptions.TolFun = 1e-005;
    startingOptions.TolX = 1e-005;
    startingOptions.Display = 'none';
    startingOptions.LargeScale ='off';
    startingOptions.MaxFunEvals = 400*(max(p)+max(q));

    Zinv = L^(-0.5)*P';
    stdData = zeros(k,k,T);
    for t=1:T
        stdData(:,:,t) = Zinv*data(:,:,t)*Zinv';
    end
    volParams = cell(k,1);
    V = zeros(T,k);
    for i=1:k
        volData = sqrt(squeeze(stdData(i,i,:)));
        [temp,~,V(:,i)] = tarch(volData,p(i),0,q(i),[],gjrType(i),[],startingOptions);
        volParams{i} = temp(2:(1+p(i)+q(i)));
    end
    if isGogarch
        startingVals = zeros(1,k*(k-1)/2)+.0001;
    end
    for i=1:k
        startingVals = [startingVals volParams{i}']; %#ok<AGROW>
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% OGARCH
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
w = .06*.94.^(0:sqrt(T));
w = w/sum(w);
if isGogarch
    offset = k*(k-1)/2;
else
    offset = 0;
end
parameters = startingVals;
ogarchOptions = optimset('fmincon');
if isGogarch
    ogarchOptions.Display = 'off';
else
    ogarchOptions.Display = 'iter';
end
ogarchOptions.Algorithm = 'sqp';
for i=1:k
    if gjrType==1
        backCast = w*abs(volData(1:length(w)));
    else
        backCast = w*(volData(1:length(w))).^2;
    end
    count = p(i) + q(i);
    LB = zeros(count,1);
    UB = LB + 1;
    A = ones(1,count);
    b = 1;
    volStart = parameters(offset + (1:count));
    volData = sqrt(squeeze(stdData(i,i,:)));
    parameters(offset + (1:count)) = fmincon(@ogarch_likelihood,volStart,A,b,[],[],LB,UB,[],ogarchOptions,volData,p(i),q(i),gjrType(i),backCast);
    offset = offset + count;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% GOGARCH
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if isGogarch
    v = length(startingVals);
    LB = zeros(1,v);
    UB = LB + 1;
    LB(1:(k*(k-1)/2)) = 1e-6;
    UB(1:(k*(k-1)/2)) = .99998*pi;
    A = zeros(k,v);
    b = .99998*ones(1,k);
    offset = k*(k-1)/2;
    for i=1:k
        count = p(i)+q(i);
        Ai = ones(1,count);
        A(i,offset + (1:count)) = Ai;
        offset = offset + count;
    end
    parameters = fmincon(@gogarch_likelihood,startingVals,A,b,[],[],LB,UB,[],options,data,p,q,gjrType,P,L,false,false);
    [ll,~,Ht] = gogarch_likelihood(parameters,data,p,q,gjrType,P,L,false,false);
else
    [ll,~,Ht] = gogarch_likelihood(parameters,data,p,q,gjrType,P,L,true,false);
end
ll = -ll;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Inference
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k2 = k*(k+1)/2;
v = k2+length(parameters);
scores = zeros(T,v);
count = 1;
for j=1:k
    for i=j:k
        scores(:,count) = squeeze(data(i,j,:) - S(i,j));
        count = count + 1;
    end
end
[~,s] = gradient_2sided(@gogarch_likelihood,parameters',data,p,q,gjrType,P,L,~isGogarch,false);
scores(:,k2+1:v) = s;
B = covnw(scores,ceil(1.2*T^(1/3)));
A = zeros(v);
A(1:k2,1:k2) = -eye(k2);
m = length(parameters);
parameters = [vech(S)' parameters]';
temp = hessian_2sided_nrows(@gogarch_likelihood,parameters,m,data,p,q,gjrType,P,L,~isGogarch,true);
A((k2+1):v,:) = temp/T;
Ainv = A\eye(v);
VCV = Ainv*B*Ainv'/T;


function voronoiStats=getVoronoiStats(Fv_cell,Vv,np,numBins)

%Replace infinite entries by NaN
Vv(isinf(Vv))=NaN;

%Allocate memory for structure
voronoiStats.ellipse=nan(numel(Fv_cell),5);
voronoiStats.ellipticity=nan(numel(Fv_cell),1);
voronoiStats.circularity=nan(numel(Fv_cell),1);
voronoiStats.angle=nan(numel(Fv_cell),1);
voronoiStats.area=nan(numel(Fv_cell),1);
voronoiStats.radii=cell(numel(Fv_cell),1);
voronoiStats.minRad=nan(numel(Fv_cell),1);
voronoiStats.maxRad=nan(numel(Fv_cell),1);
voronoiStats.meanRad=nan(numel(Fv_cell),1);

%Loop through cells to compute statistics
for q=1:1:numel(Fv_cell);

    %Get current Voronoi cell
    fv=Fv_cell{q};
    vv=Vv(fv,:);

    if ~isnan(vv(:))
        %Compute ellips and ellipticity
        [A] = ellipseFit(vv,2,np);

        a=A(3); b=A(4);
        if b<a %Oblate
            E=sqrt((a^2-b^2)./a^2);
        else %Prolate
            E=sqrt((b^2-a^2)./b^2);
        end

        %Compute radii
        x=vv(:,1);
        y=vv(:,2);
        xCentre = mean(x);
        yCentre = mean(y);
        x = (x-xCentre);
        y = (y-yCentre);
        R=sqrt(x.^2+y.^2);

        %Compute area
        vv_area=patch_area(fv,Vv);

        %Compute circularity
        C=polyCircularity(vv);

        %Compute angle
        ellipseAngle=A(end);
        ellipseAngle(ellipseAngle<0)=(2*pi+ellipseAngle(ellipseAngle<0))-pi;

        %Store in stucture
        voronoiStats.ellipse(q,:)=A;
        voronoiStats.ellipticity(q)=E;
        voronoiStats.circularity(q)=C;
        voronoiStats.angle(q)=ellipseAngle;
        voronoiStats.area(q)=vv_area;
        voronoiStats.radii{q}=R;
        voronoiStats.minRad(q)=min(R);
        voronoiStats.maxRad(q)=max(R);
        voronoiStats.meanRad(q)=mean(R);

    else %Set empty
        voronoiStats.ellipse(q,:)=nan;
        voronoiStats.ellipticity(q)=nan;
        voronoiStats.circularity(q)=nan;
        voronoiStats.angle(q)=nan;
        voronoiStats.area(q)=nan;
        voronoiStats.radii{q}=nan;
        voronoiStats.minRad(q)=nan;
        voronoiStats.maxRad(q)=nan;
        voronoiStats.meanRad(q)=nan;
    end
end

%% Compute histograms

E=voronoiStats.ellipticity;
E=E(~isnan(E));
hbE=linspace(min(E(:)),max(E(:)),numBins)';
hcE = histc(E,hbE);
hcE=hcE./size(E,1); %Normalize
voronoiStats.ellipticityHist.hc=hcE;
voronoiStats.ellipticityHist.hb=hbE;

E=voronoiStats.angle;
E=E(~isnan(E));
hbE=linspace(min(E(:)),max(E(:)),numBins)';
% hbE=linspace(0,pi,numBins)';
hcE = histc(E,hbE);
hcE=hcE./size(E,1); %Normalize
voronoiStats.angleHist.hc=hcE;
voronoiStats.angleHist.hb=hbE;

E=voronoiStats.area;
E=E(~isnan(E));
hbE=linspace(min(E(:)),max(E(:)),numBins)';
hcE = histc(E,hbE);
hcE=hcE./size(E,1); %Normalize
voronoiStats.areaHist.hc=hcE;
voronoiStats.areaHist.hb=hbE;

E=voronoiStats.circularity;
E=E(~isnan(E));
hbE=linspace(min(E(:)),max(E(:)),numBins)';
% hbE=linspace(0,1,numBins)';
hcE = histc(E,hbE);
hcE=hcE./size(E,1); %Normalize
voronoiStats.circularityHist.hc=hcE;
voronoiStats.circularityHist.hb=hbE;

E=voronoiStats.meanRad;
E=E(~isnan(E));
hbE=linspace(min(E(:)),max(E(:)),numBins)';
hcE = histc(E,hbE);
hcE=hcE./size(E,1); %Normalize
voronoiStats.meanRadHist.hc=hcE;
voronoiStats.meanRadHist.hb=hbE;

%%
% _*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 ff=lpcar2ff(ar,np)
%LPCAR2FF LPC: Convert AR coefs to complex spectrum FF=(AR,NP)

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: lpcar2ff.m,v 1.4 2007/05/04 07:01:38 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nf,p1]=size(ar);
if nargin<2 np=p1-1; end
ff=(rfft(ar.',2*np+2).').^(-1);


function a = rss_mp(alg,hyper)

%=================================================================
% RSC_MP reduced set selection
%=================================================================
% a = rss_mp(alg,hyper)
% generates a rss object, using the matching pursuit selection method
%
% hyperparameters:
% child=svm         algorithm worked on
% max_loops=1e5     maximum number of basis functions
% tolerance=1e-5    tolerated loss in ||w-w*||^2
% backfit=1         backfit on every nth iteration
% backfit_at_start=100  always backfit for first e.g. 100 iterations
% dont_revisit=1    dont return to old basis function optimization, always get new one
% reoptimize_b=1    recalculate the threshold b0
% alpha_cutoff=0    throw away svs with abs(alpha)<n
% bal_w=0           treat multiple w's as equal by normalizing by length
% optimizer='iterative' iterative update of matrix inverse
%
% model:
% alpha         new alphas for rs-vectors
% Xsv           rs vectors
% b0            the threshold
%
% stats:
% w2=0          final value of ||w-w*||^2
% res=[]        results on a separate test set
% dtst=[]       separate test set
% test_on=0     iterations to test on
%
% methods:
% train         constructs a reduced set, returns trained rs-machine
% test          tests new rs-machine on supplied data
%
% example:
% d=gen(toy2d('2circles','l=100'));
% [r,a]=train(svm({kernel('rbf',1),'C=10000','alpha_cutoff=1e-2'}),d);
% [r,a2]=train(rss_mp(a,'tolerance=1e-2'),d);
% test(a2,d,loss)
%
%=================================================================
% author: goekhan bakir, jason weston
% reference: fast binary and multi-output rss, 2004
%=================================================================

  %hyperparams
  a.child=svm;
  a.max_loops=100000;% maximum number of basis functions
  a.tolerance=1e-5;
  a.backfit=1;      % backfit on every nth iteration
  a.backfit_at_start=100; % always backfit for first e.g. 100 iterations
  a.dont_revisit=1;  % dont return to old basis function optimization, always get new one
  a.reoptimize_b=1;
  a.alpha_cutoff=0;
  a.bal_w=0;         % treat multiple w's as equal by normalizing by length
  a.optimizer='iterative'; % iterative update of matrix inverse

  % model
  a.alpha=[];
  a.Xsv=[];
  a.b0=0;

  % training / testing statistics
  a.w2=0;  % final value of ||w*-w^2||^2
  a.res=[];% results on a separate test set
  a.dtst=[];% separate test set
  a.test_on=0; % iterations to test on

  if nargin==0
    a.child=svm;
  else
    a.child=alg; %% algorithm to use
  end

  p=algorithm('rss_mp');
  a= class(a,'rss_mp',p);
  a.algorithm.use_signed_output=0;

  if nargin==2
    eval_hyper;
  end


function [ y, dzdw,dzdb,opts] = linear_layer( I,weight,bias,dzdy,opts)
%FAST_MLP_LAYER Summary of this function goes here
%   Detailed explanation goes here
%I: input_dim x batch_size

dzdw=[];
dzdb=[];

if exist('opts','var')&&isfield(opts,'use_nntoolbox')&&opts.use_nntoolbox==1
    [out,in]=size(weight);
    batch_size=size(I,2);
    %use nntoolbox
    if ~isfield(opts,'layer')||length(opts.layer)<opts.current_layer||~isfield(opts.layer{opts.current_layer},'fc_nntb')
        product_info=ver('nnet');
        opts.nnet_ver=str2double(product_info.Version);
        if opts.nnet_ver<11
            opts.layer{opts.current_layer}.fc_nntb=nnet.internal.cnn.layer.Convolution2D('conv2d_nntb', [1,1], in ,out, [1,1], [0,0]);
        else
            opts.layer{opts.current_layer}.fc_nntb=nnet.internal.cnn.layer.Convolution2D('conv2d_nntb', [1,1], in ,out, [1,1],'manual', [0,0,0,0]);
        end
        if opts.use_gpu
            opts.layer{opts.current_layer}.fc_nntb = setupForGPUPrediction(opts.layer{opts.current_layer}.fc_nntb);
        else
            opts.layer{opts.current_layer}.fc_nntb =setupForHostPrediction(opts.layer{opts.current_layer}.fc_nntb);
        end
    end
    fc_nntb=opts.layer{opts.current_layer}.fc_nntb;
    if ~isempty(bias)
        fc_nntb.Bias.Value=permute(bias(:),[3,4,1,2]);
    else
        fc_nntb.Bias.Value=zeros(1,1,out,1);
    end
    fc_nntb.Weights.Value=permute(weight,[4,3,2,1]);
    I=permute(I,[3,4,1,2]);

    if isempty(dzdy)
        y=fc_nntb.forward(I);
        y=permute(y,[3,4,1,2]);
    else

        dzdy=permute(dzdy,[3,4,1,2]);
        if opts.nnet_ver<11
            y = fc_nntb.backward(  I, [], dzdy, [] );
            gradients = fc_nntb.gradients(I, dzdy);
        else
            [y,gradients] = fc_nntb.backward( I, [], dzdy, [] );
        end

        dzdw=gradients{1}./batch_size;
        dzdb=gradients{2}./batch_size;
        y=permute(y,[3,4,1,2]);
        dzdw=permute(dzdw,[4,3,2,1]);
        dzdb=reshape(dzdb,size(bias));
    end

    return;
end

if isempty(dzdy)
    %forward mode

    y=weight*I;

    if ~isempty(bias)
        y=y+bias;
    end

else
    %backward mode

    y=weight'*dzdy;
    if ~isempty(bias)
        dzdb=mean(dzdy,2);%minibatch averaging
    end
    dzdy=permute(dzdy,[1,3,2]);
    I=permute(I,[3,1,2]);
    dzdw=dzdy.*I;
    dzdw=mean(dzdw,3);

end


function c = oned_binary_imdilate(a,b)

sza = size(a);
szb = size(b);
if nnz(sza>1) > 1 || nnz(szb>1) > 1,
  error('oned_binary_imdilate is only meant for one-dimensional arrays');
end
nb = length(b);
if mod(nb,2) == 0,
  b = [b,false];
end

c = myconv(a,b,'same',false) > 0;


function C = spm_atranspa(A)
% Multiplies the transpose of a matrix by itself
% FORMAT C = spm_atranspa(A)
% A - real matrix
% C - real symmetric matrix resulting from A'A
%_______________________________________________________________________
%
% This compiled routine was written to save both memory and CPU time but
% is now deprecated. Use A'*A directly instead.
%_______________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_atranspa.m 4418 2011-08-03 12:00:13Z guillaume $

persistent runonce
if isempty(runonce)
    warning('spm_atranspa is deprecated. Use A''*A instead.');
    runonce = 1;
end

C = A'*A;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   q = solve_spherical_wrist(robot, q, T, wrist)
%   Solves the inverse kinematic problem for a spherical wrist
%   robot: robot structure.
%   q: vector containing the values of the joints 1, 2 and 3.
%   T: orientation of the last reference system.
%   wrist: select -1 or 1 for two possible solutions (wrist up, wrist down)
%
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

function q = solve_spherical_wrist(robot, q, T, wrist, method)

switch method

     %algebraic solution
    case 'algebraic'
        T01=dh(robot, q, 1);
        T12=dh(robot, q, 2);
        T23=dh(robot, q, 3);

        Q=inv(T23)*inv(T12)*inv(T01)*T;

        %detect the degenerate case when q(5)=0, this leads to zeros
        % in Q13, Q23, Q31 and Q32 and Q33=1
        thresh=1e-12;
        %detect if q(5)==0
        % this happens when cos(q5) in the matrix Q is close to 1
        if abs(Q(3,3)-1)>thresh
            %normal solution
            if wrist==1 %wrist up
                q(4)=atan2(-Q(2,3),-Q(1,3));
                q(6)=atan2(-Q(3,2),Q(3,1));
                %q(5)=atan2(-Q(3,2)/sin(q(6)),Q(3,3));
            else %wrist down
                q(4)=atan2(-Q(2,3),-Q(1,3))-pi;
                q(6)=atan2(-Q(3,2),Q(3,1))+pi;
                %q(5)=atan2(-Q(3,2)/sin(q(6)),Q(3,3));
            end
            if abs(cos(q(6)+q(4)))>thresh
                cq5=(Q(1,1)+Q(2,2))/cos(q(4)+q(6))-1;
            end
            if abs(sin(q(6)+q(4)))>thresh
                cq5=(-Q(1,2)+Q(2,1))/sin(q(4)+q(6))-1;
            end
            if abs(sin(q(6)))>thresh
                sq5=-Q(3,2)/sin(q(6));
            end
            if abs(cos(q(6)))>thresh
                sq5=Q(3,1)/cos(q(6));
            end
            q(5)=atan2(sq5,cq5);

        else %degenerate solution, in this case, q4 cannot be determined,
             % so q(4)=0 is assigned
            if wrist==1 %wrist up
                q(4)=0;
                q(5)=0;
                q(6)=atan2(-Q(1,2)+Q(2,1),Q(1,1)+Q(2,2));
            else %wrist down
                q(4)=-pi;
                q(5)=0;
                q(6)=atan2(-Q(1,2)+Q(2,1),Q(1,1)+Q(2,2))+pi;
            end

        end

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

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

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

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

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

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

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

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

    otherwise
        disp('no method specified in solve_spherical_wrist');
end


% Helper method for quickly creating bounding boxes.
%
% Args:
%   basis2d - 2x2 matrix for the basis in the XY plane
%   centroid - 1x3 vector for the 3D centroid of the bounding box.
%   coeffs - 1x3 vector for the radii in each dimension (x, y, and z)
%
% Returns:
%   bb - a bounding box struct.
%
% Author: Nathan Silberman (silberman@cs.nyu.edu)
function bb = create_bounding_box_3d(basis2d, centroid, coeffs)
  assert(all(size(basis2d) == [2, 2]));
  assert(numel(centroid) == 3);
  assert(numel(coeffs) == 3);

  centroid = centroid(:)';
  coeffs = coeffs(:)';

  bb = struct();
  bb.basis = zeros(3,3);
  bb.basis(3,:) = [0 0 1];
  bb.basis(1:2,1:2) = basis2d;

  bb.centroid = centroid;
  bb.coeffs = coeffs;
%   bb.volume = prod(2 * bb.coeffs);
end


%NULIBSVC Trainable classifier: LIBSVM, nu-algorithme
%
% 	[W,J,NU] = NULIBSVC(A,KERNEL,NU)
% 	[W,J,NU] = A*NULIBSVC([],KERNEL,NU)
% 	[W,J,NU] = A*NULIBSVC(KERNEL,NU)
%
% INPUT
%   A	      Dataset
%   KERNEL  Mapping to compute kernel by A*MAP(A,KERNEL)
%           or string to compute kernel by FEVAL(KERNEL,A,A)
%           or cell array with strings and parameters to compute kernel by
%           FEVAL(KERNEL{1},A,A,KERNEL{2:END})
%           Default: linear kernel.
%   NU      nu value, upperbound error.
%           Default NU is derived from 1-NN error.
%
% OUTPUT
%   W       Mapping: Support Vector Classifier
%   J       Object idences of support objects. Can be also obtained as W{4}
%   NU      Actual nu_value used
%
% DESCRIPTION
% Optimizes a support vector classifier for the dataset A by the libsvm
% package, see http://www.csie.ntu.edu.tw/~cjlin/libsvm/. LIBSVC calls the
% svmtrain routine of libsvm for training. Classifier execution for a
% test dataset B may be done by D = B*W; In D posterior probabilities are
% given as computed by svmpredict using the '-b 1' option.
%
% The kernel may be supplied in KERNEL by
% - an untrained mapping, e.g. a call to PROXM like W = LIBSVC(A,PROXM([],'R',1))
% - a string with the name of the routine to compute the kernel from A
% - a cell-array with this name and additional parameters.
% This will be used for the evaluation of a dataset B by B*W or PRMAP(B,W) as
% well.
%
% If KERNEL = 0 (or not given) it is assumed that A is already the
% kernelmatrix (square). In this also a kernel matrix should be supplied at
% evaluation by B*W or PRMAP(B,W). However, the kernel has to be computed with
% respect to support objects listed in J (the order of objects in J does matter).
%
% REFERENCES
% R.-E. Fan, P.-H. Chen, and C.-J. Lin. Working set selection using the second order
% information for training SVM. Journal of Machine Learning Research 6, 1889-1918, 2005
%
% SEE ALSO (PRTools Guide)
% MAPPINGS, DATASETS, LIBSVC, SVC, PROXM

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

function [W,J,NU] = nulibsvc(varargin)

	checktoolbox('libsvm');

  mapname = 'nuLIBSVM';
  argin = shiftargin(varargin,{'prmapping','char','cell'});
  argin = setdefaults(argin,[],proxm([],'p',1),[],[]);

  if mapping_task(argin,'definition')

    W = define_mapping(argin,'untrained',mapname);

	elseif mapping_task(argin,'training')			% Train a mapping.

    [a,kernel,NU] = check_for_old_call(argin);
		if isempty(NU), NU = 2*min(max(testk(a,1),0.01),(0.8*min(classsizes(a))/size(a,1))); end
		%opt = ['-s 0 -t 4 -b 1 -c ',num2str(NU)];
    opt = ['-s 1 -t 4 -b 1 -n ',num2str(NU), ' -q'];
		islabtype(a,'crisp');
		isvaldset(a,1,2); % at least 1 object per class, 2 classes
		[m,k,c] = getsize(a);
		nlab = getnlab(a);

		K = compute_kernel(a,a,kernel);
		K = min(K,K');   % make sure kernel is symmetric
		K = [[1:m]' K];  % as libsvm wants it
	                   % call libsvm
		u = svmtrain(nlab,K,opt);
		if isempty(u)
			prwarning(1,'nulibsvc: no solution for SVM, pseudo-inverse will be used')
			W = lkc(prdataset(K(:,2:end),getlabels(a)),0);
			J = [1:m]';
			return
		end
		                 % Store the results:
   	J = full(u.SVs);
    if isequal(kernel,0)
      s = [];
      in_size = length(J);
      % in_size = 0; % to allow old and new style calls
    else
      s = a(J,:);
      in_size = k;
    end

    lablist = getlablist(a);
    W = prmapping(mfilename,'trained',{u,s,kernel,J,opt},lablist(u.Label,:),in_size,c);

		W = setname(W,mapname);
		W = setcost(W,a);

  else % Evaluation

    [a,W] = deal(argin{1:2});
    [u,s,kernel,J,opt] = getdata(W);
		m = size(a,1);

		K = compute_kernel(a,s,kernel);
    k = size(K,2);
    if k ~= length(J)
      if isequal(kernel,0)
        if (k > length(J)) &  (k >= max(J))
          % precomputed kernel; old style call
          prwarning(1,'Old style execution call: The precomputed kernel was calculated on a test set and the whole training set!')
        else
          error('Inappropriate precomputed kernel!\nFor the execution the kernel matrix should be computed on a test set and the set of support objects');
        end
      else
        error('Kernel matrix has the wrong number of columns');
      end
    else
      % kernel was computed with respect to the support objects
      % we make an approprite correction in the libsvm structure
      u.SVs = sparse((1:length(J))');
    end
    K = [[1:m]' K];  % as libsvm wants it
    %[lab,acc,d] = svmpredict(getnlab(a),K,u,'-b 1');
    [lab,acc,d] = svmpredict(ones(m,1),K,u,'-b 1');
		W = setdat(a,d,W);

	end

 return;

function K = compute_kernel(a,s,kernel)

	% compute a kernel matrix for the objects a w.r.t. the support objects s
	% given a kernel description

	if  isstr(kernel) % routine supplied to compute kernel
		K = feval(kernel,a,s);
	elseif iscell(kernel)
		K = feval(kernel{1},a,s,kernel{2:end});
	elseif ismapping(kernel)
		K = a*prmap(s,kernel);
	elseif kernel == 0 % we have already a kernel
		K = a;
	else
		error('Do not know how to compute kernel matrix')
	end

	K = +K;

return

function [a,kernel,NU] = check_for_old_call(argin)

[a,kernel,NU,par] = deal(argin{:});
if ischar(kernel) && exist(kernel,'file') ~= 2
  kernel = proxm(kernel,NU);
  NU = par;
end


  function back = my_cbct_back(proj, cg, ig, varargin)
% function back = cbct_back(proj, cg, ig, varargin)
%|
%| cone-beam backprojector for feldkamp.m
%| in
%|	proj	[ns nt na]	cone-beam projection views
%| option
%|	'use_mex' 1|2|3		default: 1 mex with loop in mex
%|					2 mex with loop in matlab
%|					3 mex with "st" data order (slower)
%|					0 no mex; use matlab
%|	'ia_skip'		default: 1
%|	'offset_source'		default: 0
%|	'nthread'		default: jf('ncore')
%| out
%|	back	[nx ny nz]	back projection result
%|
%| Copyright 2004-8-28 Jeff Fessler, University of Michigan

if nargin == 1 && streq(proj, 'test'), cbct_back_test, return, end
if nargin < 3, help(mfilename), error(mfilename), end

arg.ia_skip = 1;
arg.use_mex = 1;
arg.offset_source = 0;
arg.nthread = jf('ncore');
arg.back_call = @fdk_mex_call;
arg.scale_dang = true;
arg = vararg_pair(arg, varargin);

if arg.use_mex
	if cg.pitch ~= 0 || any(cg.source_zs ~= 0)
		fail('helix not yet supported')
	end
	back = cbct_back_mex(proj, cg.ns, cg.nt, cg.na, ...
		cg.ds, cg.dt, cg.offset_s, cg.offset_t, arg.offset_source, ...
		cg.dsd, cg.dso, cg.dfs, cg.orbit, cg.orbit_start, ...
		ig.mask_or, ig.nz, ig.dx, ig.dy, ig.dz, ...
		[ig.offset_x ig.offset_y ig.offset_z], ...
		arg.ia_skip, arg.scale_dang, ...
		arg.use_mex, arg.nthread, arg.back_call);

else

	back = cbct_back_mat(proj, cg, cg.ns, cg.nt, cg.na, ...
		cg.ds, cg.dt, cg.offset_s, cg.offset_t, arg.offset_source, ...
		cg.dsd, cg.dso, cg.dfs, cg.orbit, cg.orbit_start, ...
		cg.source_zs, ...
		ig.mask_or, ig.nz, ig.dx, ig.dy, ig.dz, ...
		[ig.offset_x ig.offset_y ig.offset_z], ...
		arg.ia_skip, arg.scale_dang);
end

end % cbct_back()

%
% cbct_back_mex()
% mex backprojector
%
function img = cbct_back_mex(proj, ns, nt, na, ...
	ds, dt, offset_s, offset_t, offset_source, ...
	dsd, dso, dfs, orbit, orbit_start, ...
	mask, nz, dx, dy, dz, offset_xyz, ia_skip, scale_dang, ...
	use_mex, nthread, back_call)

[nx ny] = size(mask);
ia_list = 1:ia_skip:na;
betas = deg2rad(orbit_start + orbit * [0:na-1] / na); % [na] source angles
proj = single(proj);
nthread = int32(nthread);

switch use_mex
case 1 % loop in mex
	proj = permute(proj, [2 1 3]); % sta -> tsa
	img = back_call('fdk,ts,back', ...
			int32([nx ny nz]), [dx dy dz], ...
			offset_xyz, uint8(mask), ...
			dso, dsd, dfs, [ds dt], [offset_s offset_t], ...
			proj(:,:,ia_list), betas(ia_list), nthread);
%	img = double6(img);
	img = permute(img, [2 3 1]); % zxy -> xyz

case 2 % loop in matlab (for testing)
	proj = permute(proj, [2 1 3]); % sta -> tsa
	img = 0;
	for ia=ia_list
		ticker(mfilename, ia, na)
		% note: 2006-5-30: replaced -dy with dy
		tmp = back_call('fdk,ts,back', ...
			int32([nx ny nz]), [dx dy dz], ...
			offset_xyz, uint8(mask), ...
			dso, dsd, dfs, [ds dt], [offset_s offset_t], ...
			proj(:,:,ia), betas(ia), nthread);
%		tmp = double6(tmp);
		img = img + tmp;
	end
	img = permute(img, [2 3 1]); % zxy -> xyz

case 3 % fdk,st (for testing only - slower!)
	warn 'fdk,st does not handle projection view edges completely'
	img = back_call('fdk,st,back', ...
			int32([nx ny nz]), [dx dy dz], ...
			offset_xyz, uint8(mask), ...
			dso, dsd, dfs, [ds dt], [offset_s offset_t], ...
			proj(:,:,ia_list), betas(ia_list), nthread);
%	img = double6(img);

otherwise
	fail 'bug'
end

if scale_dang % final "\der angle" scale:
	img = (0.5 * deg2rad(abs(orbit)) / (na/ia_skip)) * img;
end

end % cbct_back_mex()

%
% cbct_back_mat()
% matlab backprojector (slower)
%
function img = cbct_back_mat(proj, cg, ns, nt, na, ...
	ds, dt, offset_s, offset_t, offset_source, ...
	dsd, dso, dfs, orbit, orbit_start, ...
	source_zs, ...
	mask, nz, dx, dy, dz, offset_xyz, ia_skip, scale_dang)

if any(source_zs ~= 0)
	warn('helix not yet tested')
end

[nx ny] = size(mask);
betas = deg2rad(orbit_start + orbit * [0:na-1] / na); % [na] source angles

% precompute as much as possible
wx = (nx-1)/2 + offset_xyz(1);
wy = (ny-1)/2 + offset_xyz(2);
wz = (nz-1)/2 + offset_xyz(3);
[xc yc] = ndgrid(([0:nx-1] - wx) * dx, ([0:ny-1] - wy) * dy);
zc = ([0:nz-1] - wz) * dz;

if 0 % limit back-projection to FOV?  removed 2008-10-9
	rr = sqrt(xc.^2 + yc.^2); % [nx,ny]
	smax = ((ns-1)/2-abs(offset_s)) * ds; % maximum detector s coordinate

	if isinf(dfs)
		gamma_max = atan(smax/dsd);
	elseif dfs == 0
		gamma_max = smax / dsd;
	end

	rmax = dso * sin(gamma_max);
	mask = mask & (rr < rmax);
end
clear wx wy wz rr smax rmax

xc = xc(mask); % [np] pixels within mask
yc = yc(mask);

ws = (ns+1)/2 + offset_s; % trick: +1 because matlab starts from 1
wt = (nt+1)/2 + offset_t;

% loop over slices
img = zeros([size(mask) nz]);
sdim = [ns+3 nt+3]; % trick: extra zeros saves indexing in loop
proj1 = zeros(sdim);
ticker reset

% Greg Handy stuff:
num_turns = orbit/360;
halfNumAngles = round((na/num_turns)/2);
myPitch = cg.pitch * cg.nt * dso / dsd * cg.dt;
h = myPitch / (2*pi);
dist = .5 * myPitch;
zindex = 1;

for iz=1:nz

	% Greg Handy stuff:
	% Just trying to limit the number of angles used
	lower_limit = zc(iz) - dist;

	% enter into the acceptable range for the z-slices
	while zindex < size(cg.source_zs,1)+1 && cg.source_zs(zindex) < lower_limit
		zindex = zindex + 1;
	end
	lowerA = zindex - halfNumAngles;
	upperA = zindex + halfNumAngles;

	% a check to prevent error; images without enough views will be poor
	if lowerA <= 0
		lowerA = 1;
	end
	if upperA > na
		upperA = na;
	end

	% loop over each projection angle
	img2 = 0;
	for ia=lowerA:ia_skip:upperA
		ticker(mfilename, [iz ia], [nz na])
		beta = betas(ia);

		x_beta = +xc * cos(beta) + yc * sin(beta);
		y_betas = dso - (-xc * sin(beta) + yc * cos(beta));

		% detector indices
		if isinf(dsd) || isinf(dso) % par
			mag = ones(size(y_betas));
		else
			mag = dsd ./ y_betas;
		end

		if isinf(dfs) ... % flat
			|| isinf(dsd) || isinf(dso) % par
			sprime = mag .* x_beta;
		elseif (dfs == 0) % arc
			r_loop = x_beta - offset_source;
			sprime = dsd * atan2(r_loop, y_betas);
		end

		tprime = mag * (zc(iz) - source_zs(ia));
		bs = sprime / ds + ws;
		bt = tprime / dt + wt;

		% bi-linear interpolation:
		is = floor(bs); % left bin
		it = floor(bt);

		wr = bs - is;	% left weight
		wl = 1 - wr;	% right weight
		wu = bt - it;	% upper weight
		wd = 1 - wu;	% lower weight

		ibad = (is < 0) | (is > ns) | (it < 0) | (it > nt);
		is(ibad) = ns+1; % trick! point at harmless zeros
		it(ibad) = nt+1;

		proj1(1+[1:ns],1+[1:nt]) = proj(:,:,ia); % trick: left side
		p1 =	wl .* proj1(sub2ind(sdim, is+1,it+1)) + ...
			wr .* proj1(sub2ind(sdim, is+2,it+1));
		p2 =	wl .* proj1(sub2ind(sdim, is+1,it+2)) + ...
			wr .* proj1(sub2ind(sdim, is+2,it+2));

		p0 = wu .* p1 + wd .* p2; % vertical interpolation

		if isinf(dfs) ... % flat
			|| isinf(dsd) || isinf(dso) % par
			p0 = p0 .* mag.^2; % back-projection weighting for flat
		elseif dfs == 0 % arc
			p0 = p0 .* (dsd.^2) ./ (r_loop.^2 + y_betas.^2);
		end

		img2 = img2 + p0;
	end % ia

	img(:,:,iz) = embed(img2, mask);
end % iz

if scale_dang % final "\der angle" scale:
	img = (0.5 * deg2rad(abs(orbit)) / (na*ia_skip)) * img;
end

end % cbct_back_mat()

%
% fdk_mex_call()
%
function out = fdk_mex_call(varargin)
if exist('fdk_mex') == 3
	printm 'using fdk_mex'
	out = fdk_mex(varargin{:});
elseif exist('jf_mex') == 3
	out = jf_mex(varargin{:});
else
	fail('bug: neither fdk_mex nor jf_mex found')
end
end % fdk_mex_call()

%
% cbct_back_test()
% compare various versions to check consistency
%
function cbct_back_test

down = 8; % fast test
% todo: test parallel, flat, arc
%cg = ct_geom('ge1', 'nt', 800, 'na', 50*down, 'dsd', inf, 'down', down);
cg = ct_geom('ge1', 'nt', 800, 'na', 50*down, 'down', down);
%cg.dt = -cg.dt; % todo: handle this case
ig = image_geom('nx', 512, 'ny', 496, 'nz', 480, 'fov', 500, ...
	'mask', 'all-but-edge', 'down', down);

ell = [ ... % somewhat realistic phantom object
	[30 10 10	150 150 280	0 0 1000]; % 30cm diam
	[80 10 10	50 50 30	0 0 300]; % bone-like inserts
	[-10 -40 75	40 40 40	0 0 600];
	[-10 80 -20	30 30 30	0 0 600];
];

if 1
	proj = ellipsoid_proj(cg, ell);
	proj = fdk_filter(proj, 'ramp', cg.dsd, cg.dfs, cg.ds);
else
	proj = cg.zeros;
	proj(:,:,9) = 1;
end
% im clf, im(proj, 'true projections'), cbar, return

if 0 % zero outer edges of projection for comparing ,st to ,ts
	proj([1 cg.ns], :, :) = 0;
	proj(:, [1 cg.nt], :) = 0;
	f.compare_st = true;
else
	f.compare_st = false;
end

	ia_skip = 3; % stress test and makes it faster too
	args = {proj, cg, ig, 'ia_skip', ia_skip, 'scale_dang', 0};
if 1
	back0 = cbct_back(args{:}, 'use_mex', 0);
%	im(back0), return
end

if exist('jf_mex') == 3
	for ii = 1:3
		use_mex = ii;
		printm('testing jf_mex with use_mex=%d', use_mex)
		back1{ii} = cbct_back(args{:}, ...
			'use_mex', use_mex, 'back_call', @jf_mex);
	end
	printm('mpd mex1 vs mat: %g%%', max_percent_diff(back1{1}, back0))
	printm('mpd mex1 vs mex2: %g%%', max_percent_diff(back1{1}, back1{2}))
	if f.compare_st % only if edges are zeroed
	printm('mpd mex1 vs mex3: %g%%', max_percent_diff(back1{1}, back1{3}))
	end
end

if exist('fdk_mex') == 3
	printm('found fdk_mex:\n %s', which('fdk_mex'))
	back2 = cbct_back(args{:}, 'use_mex', 1, 'back_call', @fdk_mex);
	printm('mpd jf_mex vs fdk_mex: %g%%', max_percent_diff(back1{1}, back2))

	if 1
		im plc 1 3
		iz = 1:ig.nz;
		im(1, back1{1}(:,:,iz))
		im(2, back2(:,:,iz))
		err = back2(:,:,iz) - back1{1}(:,:,iz);
		im(3, err)
	end
end

end % cbct_back_test()


function ptt = pulsetransit(ecgann, pulseann)
%   ptt = pulsetransit(ecgann, pulseann)
%
%   OVERVIEW:   Calculate PTT
%               match pulse annotations with corresponding ecg annotations
%
%   INPUT:
%
%   OUTPUT:
%
%   DEPENDENCIES & LIBRARIES:
%   REFERENCE:
%	REPO:
%       https://github.com/cliffordlab/hrv_toolbox
%   ORIGINAL SOURCE AND AUTHORS:
%       Main script written by Adriana N. Vest
%       Dependent scripts written by various authors
%       (see functions for details)
%	COPYRIGHT (C) 2016
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information
%%

ptt = NaN((length(ecgann)-1),3);
for i = 1:(length(ecgann)-1)
    x = find(pulseann < ecgann(i+1) & pulseann > ecgann(i));
    ptt(i,1) = ecgann(i);
    if x > 0
        if length(x)>1
            ptt(i,2) = pulseann(x(1));
        else
            ptt(i,2) = pulseann(x);
        end
    end
    ptt(i,3) = ptt(i,2) - ptt(i,1);
end

end


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function [optval, optstd] = CallPut2(S,K,C)
% S = NSim x Nt matrix of simulated prices
% K = Strike price
% C = 1 -> Call; C = 0 -> Put

    Nt = size(S,2);         % Number of Discretization Steps
    V = S(:,Nt);
    if(C==1)
        optval = mean(max(V-K,0));
        optstd = std(max(V-K,0));
    else
        optval = mean(max(K-V,0));
        optstd = std(max(K-V,0));
    end
end


function [aVal,aJacob,aHess,papt]=aPolarCoordTurn2DOmega(x,tauInv,tauAccelInv)
%%APOLARCOORDTURN2DOMEGA The continuous-time drift function for a 2D
%           coordinated turn model with the velocity expressed in terms of
%           a heading angle and a speed rather than in terms of a Cartesian
%           velocity vector. Additionally a turn rate is part of the state
%           (unlike in aPolarLin2D) and a linear acceleration term can be
%           given, which acts in the direction of motion. The turn rate and
%           linear acceleration can optionally have time constants
%           associated with them, like in the Singer model, modelling a
%           tendancy to eventually return to non-accelerating, straight-
%           line motion.
%
%INPUTS: xState The 5X1 or 6X1 target state for 2D motion. If there is no
%               linear acceleration (acceleration along the direction of
%               motion), then the state is [xPos;yPos;theta;v;omega],
%               where (xPos,yPos) are the Cartesian position, theta is the
%               angle of the velocity vector in radians counterclockwise
%               from the x axis, v is the speed, and omega is the turn
%               rate, typically in radians per second. If a linear
%               acceleration component is provided, then the state is
%               [xPos;yPos;theta;v;omega,al].
%        tauInv The inverse of the correlation time constant tau for the
%               turn rate in seconds. tauInv must be positive. The default
%               if omitted or an empty matrix is passed is 0.
%   tauAccelInv The inverse of the correlation time constant for the linear
%               acceleration in seconds. This parameter is not used if
%               there is no linear acceleration. the default if omitted or
%               an empty matrix is passed is 0.
%
%OUTPUTS: aVal The 5X1 (or 6X1 with linear acceleration) time-derivative of
%              the state.
%       aJacob The 5X5 (or 6X6) matrix of partial derivatives of aVals
%              such that aJacob(:,k) is the partial derivative of
%              aVals(:,k) with respect to x(k).
%        aHess The 5X5X5 (or 6X6X6) matrix of second derivatives of aVals
%              such that aHess(:,k1,k2) is the second partial derivative of
%              aVals with respect to x(k1) and x(k2).
%         papt The 5X1 or 6X1 derivative with resect to time of aVals.
%              This is all zeros, because the model is time invariant.
%
%The basic 2D coordinated turn model in Cartesian coordinates is described
%in Section VA of [1]. When the turn rate is something that must be
%estimated, it is assumed that the continuous-time turn rate model is
%omegaDot=-(1/tauTurn)*Omega+noise
%Note that the ordering of the state elements assumed by this function
%differs from the ordering of the state elements assumed in [1].
%
%The 2D coordinates turn model in Cartesian coordinates is also described
%in Chapter 4.2.3 of [2].
%
%The corresponding diffusion matrix is given by the function DCoordTurn2D.
%The corresponding discrete-time functions are FCoordTurn2D and
%QCoordTurn. However, note that the discrete-time functions are
%direct-discrete models and not discretizations of the continuous-time
%models as the propagated PDF does not remain Gaussian over time.
%
%REFERENCES:
%[1] X. R. Li and V. P. Jilkov, "Survey of maneuvering target tracking.
%    Part I: Dynamic models," IEEE Transactions on Aerospace and Electronic
%    Systems, vol. 39, no. 4, pp. 1333-1364, Oct. 2003.
%[2] S. Blackman and R. Popoli, Design and Analysis of Modern Tracking
%    Systems. Norwood, MA: Artech House, 1999.
%
%January 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<2||isempty(tauInv))
    tauInv=0;
end

if(nargin<3||isempty(tauAccelInv))
    tauAccelInv=0;
end

%Extract the heading and speed (polar velocity components).
theta=x(3);
v=x(4);
omega=x(5);%The turn rate

xDim=size(x,1);

sinTheta=sin(theta);
cosTheta=cos(theta);
switch(xDim)
    case 5%There is no linear acceleration
        %From Equations 61 and 67 in Li's paper.
        aVal=[v*cosTheta;%Position dervative
              v*sinTheta;%Position derivative
              omega;%Heading derivative
              0;%Speed derivative
              -tauInv*omega];%Turn rate derivative

        if(nargout>1)
            dXdTheta=-v*sinTheta;
            dYdTheta=v*cosTheta;
            dXdv=cosTheta;
            dYdv=sinTheta;

            aJacob=[0,0,dXdTheta,dXdv,0;
                    0,0,dYdTheta,dYdv,0;
                    0,0,0,          0,1;
                    0,0,0,          0,0;
                    0,0,0,          0,-tauInv];

            if(nargout>2)
                aHess=zeros(5,5,5);

                dXdThetadTheta=-v*cosTheta;
                dXdvdTheta=-sinTheta;

                dXdThetadv=dXdvdTheta;
                dXdvdv=0;

                %%%
                dYdThetadTheta=-v*sinTheta;
                dYdvdTheta=cosTheta;

                dYdThetadv=dYdvdTheta;
                dYdvdv=0;

                aHess(:,:,3)=[0,0,dXdThetadTheta,dXdvdTheta,0;
                              0,0,dYdThetadTheta,dYdvdTheta,0;
                              zeros(3,5)];
                aHess(:,:,4)=[0,0,dXdThetadv,dXdvdv,0;
                              0,0,dYdThetadv,dYdvdv,0;
                              zeros(3,5)];

                if(nargout>3)
                    papt=zeros(5,1);
                end
            end
        end
    case 6%There is a linear acceleration component.
        al=x(6);%The linear acceleration (vDot)

        aVal=[v*cosTheta;%Position dervative
              v*sinTheta;%Position derivative
              omega;%Heading derivative
              al;%Speed derivative
              -tauInv*omega;%Turn rate derivative
              -tauAccelInv*al];%Linear acceleration derivative

        if(nargout>1)
            dXdTheta=-v*sinTheta;
            dYdTheta=v*cosTheta;
            dXdv=cosTheta;
            dYdv=sinTheta;

            aJacob=[0,0,dXdTheta,dXdv,        0,0;
                    0,0,dYdTheta,dYdv,        0,0;
                    0,0,       0,   0,        1,0;
                    0,0,       0,   0,        0,1;
                    0,0,       0,   0,  -tauInv,0;
                    0,0,       0,   0,        0,-tauAccelInv];

            if(nargout>2)
                aHess=zeros(6,6,6);

                dXdThetadTheta=-v*cosTheta;
                dXdvdTheta=-sinTheta;

                dXdThetadv=dXdvdTheta;
                dXdvdv=0;

                %%%
                dYdThetadTheta=-v*sinTheta;
                dYdvdTheta=cosTheta;

                dYdThetadv=dYdvdTheta;
                dYdvdv=0;

                aHess(:,:,3)=[0,0,dXdThetadTheta,dXdvdTheta,0,0;
                              0,0,dYdThetadTheta,dYdvdTheta,0,0;
                              zeros(4,6)];

                aHess(:,:,4)=[0,0,dXdThetadv,dXdvdv,0,0;
                              0,0,dYdThetadv,dYdvdv,0,0;
                              zeros(4,6)];

                if(nargout>3)
                    papt=zeros(6,1);
                end
            end
        end
    otherwise
        error('The dimensionality of the state is neither 5 nor 6.');
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function FaceMod(FNameOut)

[FName, FPath]=uigetfile('*.bmp','Choose a file to modify:');

img=imread(FName);
Msk=imread('MaskF.bmp')/255;
MskI=imread('MaskFi.bmp')/255;

imgMsk=img.*Msk;
imgMsk=img.*MskI;

Norm=100/mean(mean(imgMsk));
imgNorm=imgMsk*Norm;
disp(mean(mean(imgNorm)));
imwrite(imgNorm(:,:),sprintf('%s%s%s%s%s%s',FNameOut,'O','1','_F','0','.bmp'),'BMP');
imwrite(flipdim(imgNorm(:,:),1),sprintf('%s%s%s%s%s%s',FNameOut,'O','2','_F','0','.bmp'),'BMP');

FSize=[1 2 4 8 16 32 64 128];
for i=1:8;

    h = fspecial('average', [FSize(i),FSize(i)]);
    imgFiltMsk=imfilter(imgNorm,h).*Msk;
    Norm=100/mean(mean(imgFiltMsk));
    imgFiltNorm=imgFiltMsk*Norm;
    disp(mean(mean(imgFiltNorm)));

    imwrite(imgFiltNorm,sprintf('%s%s%s%s%s%s',FNameOut,'O','1','_F',num2str(i),'.bmp'),'BMP');
    imwrite(flipdim(imgFiltNorm,1),sprintf('%s%s%s%s%s%s',FNameOut,'O','2','_F',num2str(i),'.bmp'),'BMP');
end


function [model, cost_curve] = badacostWithTreesTrain( X0, X1, L1, varargin )
%--------------------------------------------------------------------------
% function [model, cost_curve] = badacostWithTreesTrain( X0, X1, L1, varargin)
%
% This function performs Multiclass Bosting ADApted for Costs (badacost)
% algorithm with trees as weak learner.
%
% INPUTS
%  X0         - [N0xF] negative feature vectors
%  X1         - [N1xF] positive feature vectors
%  L1         - [N1x1] positive subclasses labels  (in [1, H] where H is the #subclasses)
%  pBoost     - additional params (struct or name/value pairs)
%   .nWeak      - [128] number of trees to learn
%   .shrinkage  - [0.1] learning rate (multiplicative factor for weak learner weights).
%   .resampling - [0.1] sampling proportion of data for weak learner training.
%   .Cost       - [onesH+1,H+1)-eye(H+1))] C(i, j) is the cost of classifing in j whereas the
%                 real label is i.
%   .stopAtNegWeight [1] Stop iterating badacost when a Weak Learner gets a negative weight
%   .maxDepth   - [] Max depth levels of the decision trees.
%   .quantized  - [0]
%   .use_rus    - [0] Use Random Under Sampling.
%   .verbose    - [0] if true print status information
%
% OUTPUTS
%  model      - learned boosted tree classifier w the following fields
%   .fids       - [K x nWeak] feature ids for each node
%   .thrs       - [K x nWeak] threshold corresponding to each fid
%   .child      - [K x nWeak] index of child for each node (1-indexed)
%   .hs         - [K x nWeak] log ratio (.5*log(p/(1-p)) at each node
%   .weights    - [K x nWeak] total sample weight at each node
%   .depth      - [K x nWeak] depth of each node
%   .errs       - [1 x nWeak] error for each tree (for debugging)
%   .losses     - [1 x nWeak] loss after every iteration (for debugging)
%   .treeDepth  - depth of all leaf nodes (or 0 if leaf depth varies)
%

%
% Author: Jose M. Buenaposada

x = [X0; X1];

% We make 1 the negative class label and add 1 to the rest of labels
y = [ones(size(X0,1), 1); L1(:) + 1];
num_classes = length(unique(y));

% get additional parameters
Cost = ones(num_classes, num_classes) - diag(ones(num_classes, 1));
dfs={ 'nWeak',128, 'shrinkage', 0.1, 'resampling', 0.5, 'Cost', Cost, 'stopAtNegWeight', 1, ...
      'fracFtrs', 1, 'minDepth', [], 'maxDepth', [], 'quantized', 0, 'variable_depth', 0, ...
      'use_rus', 0, ...
      'verbose', 0};
[nWeak,shrinkage,resampling,Cost,stopAtNegWeight,fracFtrs,minDepth,maxDepth,quantized,variable_depth,use_rus,verbose]=getPrmDflt(varargin,dfs,1);

opt.SET_MINPARENT_TREE_WL = 1;
opt.learning_rate         = shrinkage;
opt.frac_features         = fracFtrs;
opt.num_iterations        = nWeak;
opt.scalar_class_labels   = 1;
opt.stop_with_negative_wl_weight = stopAtNegWeight;
opt.maxDepth              = maxDepth;
opt.minDepth              = minDepth;
opt.USE_QUANTIZED         = quantized;
opt.use_variable_depth    = variable_depth;
opt.USE_RUS_BALANCED_WL   = use_rus; % Use Random Under Sampling Balancing .
opt.verbose               = verbose;
if opt.USE_QUANTIZED
  opt.train_weak_learner    = @train_costs_pdollar_quantized_multiclasstree;
else
  opt.train_weak_learner    = @train_costs_pdollar_multiclasstree;
end
opt.classify_weak_learner = @classify_costs_pdollar_multiclasstree;
if ((resampling >= 1.0) || (resampling < 0))
  opt.USE_SAMPLED_WL        = 0;
  opt.USE_RUS_BALANCED_WL   = 0;
  opt.SAMPLE_PROPORTION_WL  = resampling;
elseif ~opt.USE_RUS_BALANCED_WL
  opt.USE_SAMPLED_WL        = 1;
  opt.SAMPLE_PROPORTION_WL  = resampling;
else
  opt.USE_SAMPLED_WL        = 0;
  opt.SAMPLE_PROPORTION_WL  = resampling;
end

msg='Training BAdaCost: nWeak=%3i shrinkage=%f resampling=%f fracFtrs=%f\n';
if(verbose), fprintf(msg,nWeak,shrinkage,resampling,fracFtrs); start=clock; end

[classfr, cost_curve] = badacostTrain( x', y, Cost, opt );
%figure;
%plot(cost_curve);
%xlabel('#weak learners');
%ylabel('cost');

% Now we adapt the output of the badacost to the P.Dollar toolbox.
% create output model struct
k=0;
nWeak = length(classfr.WEAK_LEARNERS);
for i=1:nWeak
   k=max(k,size(classfr.WEAK_LEARNERS{i}.TREE.fids,1));
end
Z = @(type) zeros(k,nWeak,type);
model=struct( 'fids',Z('uint32'), 'thrs',Z('single'), ...
  'child',Z('uint32'), 'hs',Z('single'), 'weights',Z('single'), ...
  'depth',Z('uint32')); %, 'errs',errs, 'losses',losses );
for i=1:nWeak
  T=classfr.WEAK_LEARNERS{i}.TREE;
  k=size(T.fids,1);
  model.fids(1:k,i)=T.fids;
  model.thrs(1:k,i)=T.thrs;
  model.child(1:k,i)=T.child;
  model.hs(1:k,i)=T.hs;
%  model.weights(1:k,i)=T.weights;
  model.depth(1:k,i)=T.depth;
end
depth = max(model.depth(:));
model.treeDepth = depth * uint32(all(model.depth(~model.child)==depth));
model.num_classes = classfr.num_classes;
model.Cprime      = classfr.Cprime;
model.Y           = classfr.Y;
model.wl_weights  = classfr.WEIGHTS;
model.weak_learner_type = 'trees';

% output info to log
predicted = badacostWithTreesApply(x, model);
error = sum(predicted(:) ~= y(:))/length(predicted);
fp    = sum((predicted(:) ~= 1) & (y == 1))./length(predicted);
fn    = sum((predicted(:) == 1) & (y ~= 1))./length(predicted);
msg='Done training err=%.4f fp=%.4f fn=%.4f (t=%.1fs).\n';
if(verbose)
  fprintf(msg,error,fp,fn,etime(clock,start));
end

end