function options = rbfcreate(x, y, varargin)
%RBFCREATE Creates an RBF interpolation
%   OPTIONS = RBFSET(X, Y, 'NAME1',VALUE1,'NAME2',VALUE2,...) creates an
%   radial base function interpolation
%
%   RBFCREATE with no input arguments displays all property names and their
%   possible values.
%
%RBFCREATE PROPERTIES
%

%
% Alex Chirokov, alex.chirokov@gmail.com
% 16 Feb 2006
tic;
% Print out possible values of properties.
if (nargin == 0) & (nargout == 0)
  fprintf('               x: [ dim by n matrix of coordinates for the nodes ]\n');
  fprintf('               y: [   1 by n vector of values at nodes ]\n');
  fprintf('     RBFFunction: [ gaussian  | thinplate | cubic | multiquadrics | {linear} ]\n');
  fprintf('     RBFConstant: [ positive scalar     ]\n');
  fprintf('       RBFSmooth: [ positive scalar {0} ]\n');
  fprintf('           Stats: [ on | {off} ]\n');
  fprintf('\n');
  return;
end
Names = [
    'RBFFunction      '
    'RBFConstant      '
    'RBFSmooth        '
    'Stats            '
];
[m,n] = size(Names);
names = lower(Names);

options = [];
for j = 1:m
  options.(deblank(Names(j,:))) = [];
end

%**************************************************************************
%Check input arrays
%**************************************************************************
[nXDim nXCount]=size(x);
[nYDim nYCount]=size(y);

if (nXCount~=nYCount)
  error(sprintf('x and y should have the same number of rows'));
end;

if (nYDim~=1)
  error(sprintf('y should be n by 1 vector'));
end;

options.('x')           = x;
options.('y')           = y;
%**************************************************************************
%Default values
%**************************************************************************
options.('RBFFunction') = 'linear';
options.('RBFConstant') = (prod(max(x')-min(x'))/nXCount)^(1/nXDim); %approx. average distance between the nodes
options.('RBFSmooth')   = 0;
options.('Stats')       = 'off';

%**************************************************************************
% Argument parsing code: similar to ODESET.m
%**************************************************************************

i = 1;
% A finite state machine to parse name-value pairs.
if rem(nargin-2,2) ~= 0
  error('Arguments must occur in name-value pairs.');
end
expectval = 0;                          % start expecting a name, not a value
while i <= nargin-2
  arg = varargin{i};

  if ~expectval
    if ~isstr(arg)
      error(sprintf('Expected argument %d to be a string property name.', i));
    end

    lowArg = lower(arg);
    j = strmatch(lowArg,names);
    if isempty(j)                       % if no matches
      error(sprintf('Unrecognized property name ''%s''.', arg));
    elseif length(j) > 1                % if more than one match
      % Check for any exact matches (in case any names are subsets of others)
      k = strmatch(lowArg,names,'exact');
      if length(k) == 1
        j = k;
      else
        msg = sprintf('Ambiguous property name ''%s'' ', arg);
        msg = [msg '(' deblank(Names(j(1),:))];
        for k = j(2:length(j))'
          msg = [msg ', ' deblank(Names(k,:))];
        end
        msg = sprintf('%s).', msg);
        error(msg);
      end
    end
    expectval = 1;                      % we expect a value next

  else
    options.(deblank(Names(j,:))) = arg;
    expectval = 0;
  end
  i = i + 1;
end

if expectval
  error(sprintf('Expected value for property ''%s''.', arg));
end

%**************************************************************************
% Creating RBF Interpolatin
%**************************************************************************

switch lower(options.('RBFFunction'))
      case 'linear'
        options.('rbfphi')   = @rbfphi_linear;
      case 'cubic'
        options.('rbfphi')   = @rbfphi_cubic;
      case 'multiquadric'
        options.('rbfphi')   = @rbfphi_multiquadrics;
      case 'thinplate'
        options.('rbfphi')   = @rbfphi_thinplate;
      case 'gaussian'
        options.('rbfphi')   = @rbfphi_gaussian;
    otherwise
        options.('rbfphi')   = @rbfphi_linear;
end

phi       = options.('rbfphi');

A=rbfAssemble(x, phi, options.('RBFConstant'), options.('RBFSmooth'));

b=[y'; zeros(nXDim+1, 1)];

%inverse
rbfcoeff=A\b;

%SVD
% [U,S,V] = svd(A);
%
% for i=1:1:nXCount+1
%     if (S(i,i)>0) S(i,i)=1/S(i,i); end;
% end;
% rbfcoeff = V*S'*U*b;

options.('rbfcoeff') = rbfcoeff;

if (strcmp(options.('Stats'),'on'))
    fprintf('%d point RBF interpolation was created in %e sec\n', length(y), toc);
    fprintf('\n');
end;

function [A]=rbfAssemble(x, phi, const, smooth)
[dim n]=size(x);
A=zeros(n,n);
for i=1:n
    for j=1:i
        r=norm(x(:,i)-x(:,j));
        temp=feval(phi,r, const);
        A(i,j)=temp;
        A(j,i)=temp;
    end
    A(i,i) = A(i,i) - smooth;
end
% Polynomial part
P=[ones(n,1) x'];
A = [ A      P
      P' zeros(dim+1,dim+1)];

%**************************************************************************
% Radial Base Functions
%**************************************************************************
function u=rbfphi_linear(r, const)
u=r;

function u=rbfphi_cubic(r, const)
u=r.*r.*r;

function u=rbfphi_gaussian(r, const)
u=exp(-0.5*r.*r/(const*const));

function u=rbfphi_multiquadrics(r, const)
u=sqrt(1+r.*r/(const*const));

function u=rbfphi_thinplate(r, const)
u=r.*r.*log(r+1);


function [y,ty]=v_correlogram(x,inc,nw,nlag,m,fs)
%V_CORRELOGRAM calculate correlogram [y,ty]=(x,inc,nw,nlag,m,fs)
% Usage:
%        do_env=1; do_hp=1;                            % flags to control options
%        [b,a,fx,bx,gd]=v_gammabank(25,fs,'',[80 5000]); % determine v_filterbank
%        y=v_filterbank(b,a,s,gd);                       % filter signal s
%        if do_env
%            [bl,al]=butter(3,2*800/fs);
%            y=filter(bl,al,v_teager(y,1),[],1);           % low pass envelope @ 800 Hz
%        end
%        if do_hp
%            y=filter(fir1(round(16e-3*fs),2*64/fs,'high'),1,y,[],1);  % HP filter @ 64 Hz
%        end
%        v_correlogram(y,round(10e-3*fs),round(16e-3*fs),round(12e-3*fs),'',fs);
%
% Inputs:
%        x(*,chan)  is the output of a filterbank (e.g. v_filterbank)
%                   with one column per filter channel
%        inc        frame increment in samples
%        nw         window length in samples [or window function]
%        nlag       number of lags to calculate
%        m          mode string:
%               'h' = Hamming window
%        fs         sample freq (affects only plots)
%
% Outputs:
%        y(lag,chan,frame) is v_correlogram. Lags are 1:nlag samples
%        ty                is time of the window energy centre for each frame
%                            x(n) is at time n
%
% For each channel, the calculated correlation for frame n comprises
%       y(t+1,*,n+1)=(win.*x(n*inc+(1:nw))'*x(n*inc+t+(1:nw))/sqrt(win'*abs(x(n*inc+(1:nw))).^2 * win'*abs(x(n*inc+t+(1:nw))).^2)
% This corresponds to the expression in (1.7) of [1] but incorporating the normalization from (1) of [2].
%
% Future planned mode options:
%       'd' = subtract DC component
%       'n' = unnormalized
%       'v' = variable analysis window proportional to lag
%       'p' = output the peaks only
%
% Refs:
% [1]	D. Wang and G. J. Brown. Fundamentals of computational auditory scene analysis.
%       In D. Wang and G. Brown, editors, Computational Auditory Scene Analysis: Principles,
%       Algorithms, and Applications, chapter 1. Wiley, Oct. 2006. doi: 10.1109/9780470043387.ch1
% [2]	S. Granqvist and B. Hammarberg. The correlogram: a visual display of periodicity. J. Acoust. Soc. Amer., 114: 2934, 2003.

%      Copyright (C) Mike Brookes 2011-2018
%      Version: $Id: v_correlogram.m 10867 2018-09-21 17:35:59Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
memsize=v_voicebox('memsize'); 	% set memory size to use
[nx,nc]=size(x);                % number of sampes and channels
if nargin<6
    fs=1;                       % default sample frequency is 1 Hz
    if nargin<5
        m='h';                  % default analysis window is Hamming
        if nargin<4
            nlag=[];
            if nargin<3
                nw=[];
                if nargin<2
                    inc=[];
                end
            end
        end
    end
end
if ~numel(inc)
    inc=128;                    % default frame hop is 128 samples
end
if ~numel(nw)
    nw=inc;                     % default analysis window length is the fame increment
end
nwin=length(nw);
if nwin>1                       % nw specifies the window function explicitly
    win=nw(:);
else                            % nw gives the window length
    nwin=nw;
    if any(m=='h')
        win=v_windows(3,nwin)'; % Hamming window
    else
        win=ones(nwin,1);       % window function
    end
end
if ~numel(nlag)
    nlag=nwin;
end
nwl=nwin+nlag-1;
nt=pow2(nextpow2(nwl));         % transform length
nf=floor((nx-nwl+inc)/inc);     % number of frames
i1=repmat((1:nwl)',1,nc)+repmat(0:nx:nx*nc-1,nwl,1);
nb=min(nf,max(1,floor(memsize/(16*nwl*nc))));    % chunk size for calculating
nl=ceil(nf/nb);                  % number of chunks
jx0=nf-(nl-1)*nb;                % size of first chunk in frames
wincg=(1:nwin)*win.^2/(win'*win); % determine window centre of energy
fwin=conj(fft(win,nt,1));       % conjugate fft of window
y=zeros(nlag,nc,nf);
% first do partial chunk
jx=jx0;
x2=zeros(nwl,nc*jx);
x2(:)=x(repmat(i1(:),1,jx)+repmat((0:jx-1)*inc,nwl*nc,1));
% the next line was previously: v=ifft(conj(fft(x2(1:nwin,:),nt,1)).*fft(x2,nt,1));
v=ifft(conj(fft(x2(1:nwin,:).*repmat(win(:),1,nc*jx),nt,1)).*fft(x2,nt,1));                 % v(1:nlag,:) contains second half of xcorr(x2) output
w=max(real(ifft(fwin(:,ones(1,nc*jx)).*fft(x2.*conj(x2),nt,1))),0); % v(1:nlag,:) contains second half of xcorr(|x2|^2,win) output
w=sqrt(w(1:nlag,:).*w(ones(nlag,1),:));
if isreal(x)
    y(:,:,1:jx)=reshape(real(v(1:nlag,:))./w,nlag,nc,jx); % note: some values may be NaN is x=0 throughout the window
else
    y(:,:,1:jx)=reshape(conj(v(1:nlag,:))./w,nlag,nc,jx);
end
% now do remaining chunks
x2=zeros(nwl,nc*nb);
for il=2:nl
    ix=jx+1;            % first frame in this chunk
    jx=jx+nb;           % last frame in this chunk
    x2(:)=x(repmat(i1(:),1,nb)+repmat((ix-1:jx-1)*inc,nwl*nc,1));
    % the next line was previously: v=ifft(conj(fft(x2(1:nwin,:),nt,1)).*fft(x2,nt,1));
    v=ifft(conj(fft(x2(1:nwin,:).*repmat(win(:),1,nc*nb),nt,1)).*fft(x2,nt,1));
    w=max(real(ifft(fwin(:,ones(1,nc*nb)).*fft(x2.*conj(x2),nt,1))),0);
    w=sqrt(w(1:nlag,:).*w(ones(nlag,1),:));
    if isreal(x)
        y(:,:,ix:jx)=reshape(real(v(1:nlag,:))./w,nlag,nc,nb);
    else
        y(:,:,ix:jx)=reshape(conj(v(1:nlag,:))./w,nlag,nc,nb);
    end
end
ty=(0:nf-1)'*inc+wincg;       % calculate times of window centres
if ~nargout
    imagesc(ty/fs,(1:nlag)/fs,squeeze(mean(abs(y),2)));
    if nargin<6
        us='samp';
    else
        us='s';
    end
    xlabel(['Time (' v_xticksi us ')']);
    ylabel(['Lag (' v_yticksi us ')']);
    axis 'xy';
    v_colormap('v_thermliny');
    colorbar;
    v_cblabel('Mean Correlation');
    title('Summary Correlogram');
end


function segment_length = p11_boundary_segment_length ( segment_index, h )

%*****************************************************************************80
%
%% P11_BOUNDARY_SEGMENT_LENGTH returns boundary segment lengths in problem 11.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer SEGMENT_INDEX, the index of one of the boundary segments.
%
%    Input, real H, the suggested spacing between points.
%
%    Output, integer SEGMENT_LENGTH, the number of points in the segment.
%
  if ( h <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P11_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );
    fprintf ( 1, '  Nonpositive H = %f\n', h );
    error ( 'P11_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );
  end

  if ( segment_index == 1 )

    n = round ( 4.0 / h );
    n = max ( n, 5 );
    segment_length = n + mod ( 4 - mod ( n - 1, 4 ), 4 );

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'P11_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );
    fprintf ( 1, '  Illegal SEGMENT_INDEX = %d\n', segment_index );
    error ( 'P11_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );

  end

  return
end


function [omega,u,eqn,info] = fourCurl3P2(node,elem,bdFlag,pde,option)

%% fourCurl3P2: the fourth order curl problem in 3D
%
% [w,u,eqn,info] = fourCurl3P2(node,elem,bdFlag,pde,option)
% uses the first P2 elements to approximate the velocity u and
% the stream function w.
%
% We solve the following equations:
%  -w + curl curl u = 0
%  curl curl w + u  = f
% u\times n = (curl u )\times n  = 0  on \partial \Omega
%
% please check fourCurl3doc for details.
%
% Lin Zhong, June 2013.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Set up optional input arguments
if ~exist('bdFlag','var'), bdFlag = []; end
if ~exist('option','var'), option = []; end

%% Sort elem to asend ordering
elemunSort = elem;
[elem,bdFlag] = sortelem3(elemunSort,bdFlag);

%% Construct Data Structure
[Dlambda,volume] = gradbasis3(node,elem);
volume = abs(volume);
%---------------------------------------------%
% locEdge = [1 2; 1 3; 1 4; 2 3; 2 4; 3 4];
% locFace = [2 3 4; 1 3 4; 1 2 4; 1 2 3];
locBasesIdx = [1 2 0; 1 3 0; 1 4 0; 2 3 0; 2 4 0; 3 4 0; ... % phi
               1 2 0; 1 3 0; 1 4 0; 2 3 0; 2 4 0; 3 4 0; ... % psi
               3 2 4; 3 1 4; 2 1 4; 2 1 3; ...
               4 2 3; 4 1 3; 4 1 2; 3 1 2]; % chi

%---------------------------------------------%
[elem2edge,edge] = dof3edge(elem);
[elem2face,face] = dof3face(elem);
%---------------------------------------------%
NE = size(edge,1);
NF = size(face,1);
NT = size(elem,1);
Ndof = 2*(NE+NF);
%---------------------------------------------%
elem2dof = [elem2edge elem2edge+NE elem2face+2*NE elem2face+2*NE+NF];
face2edge = zeros(NF,3,'int32');
face2edge(elem2face(:,1),:) = elem2edge(:,[4 5 6]);
face2edge(elem2face(:,2),:) = elem2edge(:,[2 3 6]);
face2edge(elem2face(:,3),:) = elem2edge(:,[1 3 5]);
face2edge(elem2face(:,4),:) = elem2edge(:,[1 2 4]);

tic;

%% Assemble Matrix
DiDjcross = zeros(NT,3,4,4);
for i = 1:4
    for j = i+1:4
        DiDjcross(:,:,i,j) = mycross(Dlambda(:,:,i),Dlambda(:,:,j),2);
        DiDjcross(:,:,j,i) = -DiDjcross(:,:,i,j);
    end
end
DiDj = zeros(NT,4,4);
for i = 1:4
    for j = i:4
        DiDj(:,i,j) = dot(Dlambda(:,:,i),Dlambda(:,:,j),2);
        DiDj(:,j,i) = DiDj(:,i,j);
    end
end

ii = zeros(210*NT,1); jj = zeros(210*NT,1);
index = 0;
for i = 1:20
    for j = i:20
        ii(index+1:index+NT) = double(elem2dof(:,i));
        jj(index+1:index+NT) = double(elem2dof(:,j));
        i1 = locBasesIdx(i,1); i2 = locBasesIdx(i,2); i3 = locBasesIdx(i,3);
        j1 = locBasesIdx(j,1); j2 = locBasesIdx(j,2); j3 = locBasesIdx(j,3);
        Aij = zeros(NT,1);  Mij = zeros(NT,1);
        %% curl-curl matrix
        if (i<=6) && (j<=6)
            Aij = 4*dot(DiDjcross(:,:,i1,i2),DiDjcross(:,:,j1,j2),2);
        end
        if (i<=6) && (j>12)
            Aij = dot(DiDjcross(:,:,i1,i2),...
            DiDjcross(:,:,j1,j3)-DiDjcross(:,:,j1,j2)+2*DiDjcross(:,:,j2,j3),2)/2;
        end
        if (i>12) && (j>12)
        % curl chi_i =  Dlambda_{i1}mycross phi_{i2,i3} + lambda_{i1}curl phi_{i2,i3}
        % curl chi_j =  Dlambda_{j1}mycross phi_{j2,j3} + lambda_{j1}curl phi_{j2,j3}
        % (Dlambda_{i1}mycross phi_{i2,i3}) dot (Dlambda_{j1}mycross phi_{j2,j3})
            temp11 = ((1+(i2==j2))*dot(DiDjcross(:,:,i1,i3),DiDjcross(:,:,j1,j3),2) ...
                    - (1+(i2==j3))*dot(DiDjcross(:,:,i1,i3),DiDjcross(:,:,j1,j2),2) ...
                    - (1+(i3==j2))*dot(DiDjcross(:,:,i1,i2),DiDjcross(:,:,j1,j3),2) ...
                    + (1+(i3==j3))*dot(DiDjcross(:,:,i1,i2),DiDjcross(:,:,j1,j2),2))/20;
            % lambda_{i1}curl phi_{i2,i3} dot lambda_{j1}curl phi_{j2,j3}
            temp22 = (1+(i1==j1))/5*dot(DiDjcross(:,:,i2,i3),DiDjcross(:,:,j2,j3),2);
            % Dlambda_{i1}mycross phi_{i2,i3} dot lambda_{j1}curl phi_{j2,j3}
            temp12 = dot( (1+(j1==i2))*DiDjcross(:,:,i1,i3) ...
                        - (1+(j1==i3))*DiDjcross(:,:,i1,i2), ...
                                       DiDjcross(:,:,j2,j3),2)/10;
            % Dlambda_{j1}mycross phi_{j2,j3} dot lambda_{i1}curl phi_{i2,i3}
            temp21 = dot( (1+(i1==j2))*DiDjcross(:,:,j1,j3) ...
                        - (1+(i1==j3))*DiDjcross(:,:,j1,j2), ...
                                       DiDjcross(:,:,i2,i3),2)/10;
            Aij = temp11 + temp22 + temp12 + temp21;
        end
        if (i<=6) && (j<=6)
            % block 1: (phi_i,phi_j)
            Mij = 1/20*((1+(i1==j1))*DiDj(:,i2,j2) ...
                      - (1+(i1==j2))*DiDj(:,i2,j1) ...
                      - (1+(i2==j1))*DiDj(:,i1,j2) ...
                      + (1+(i2==j2))*DiDj(:,i1,j1));
        end
        if (i<=6) && (7<=j) && (j<=12)
            % block 2: (psi_j,phi_i)
            Mij = 1/20*( (1+(j1==i1))*DiDj(:,j2,i2) ...
                       - (1+(j1==i2))*DiDj(:,j2,i1) ...
                       + (1+(j2==i1))*DiDj(:,j1,i2) ...
                       - (1+(j2==i2))*DiDj(:,j1,i1));

        end
        if (7<=i) && (i<=12) && (7<=j) && (j<=12)
            % block 3: (psi_j,psi_i)
            Mij = 1/20*((1+(i1==j1))*DiDj(:,i2,j2) ...
                      + (1+(i1==j2))*DiDj(:,i2,j1) ...
                      + (1+(i2==j1))*DiDj(:,i1,j2) ...
                      + (1+(i2==j2))*DiDj(:,i1,j1));
        end
        if (i<=6) && (j>12)
            % block 4: (chi_j,phi_i)
            Mij = intlambda([j1,i1,j2],3)*DiDj(:,i2,j3) ...
                 -intlambda([j1,i1,j3],3)*DiDj(:,i2,j2) ...
                 -intlambda([j1,i2,j2],3)*DiDj(:,i1,j3) ...
                 +intlambda([j1,i2,j3],3)*DiDj(:,i1,j2);
        end
        if (7<=i) && (i<=12) && (j>12)
            % block 5: (chi_j,psi_i)
            Mij = intlambda([j1,i1,j2],3)*DiDj(:,i2,j3) ...
                 -intlambda([j1,i1,j3],3)*DiDj(:,i2,j2) ...
                 +intlambda([j1,i2,j2],3)*DiDj(:,i1,j3) ...
                 -intlambda([j1,i2,j3],3)*DiDj(:,i1,j2);
        end
        if (i>12) && (j>12)
            % block 6: (chi_j,chi_i)
            Mij = intlambda([i1,j1,i2,j2],3)*DiDj(:,i3,j3) ...
                 -intlambda([i1,j1,i2,j3],3)*DiDj(:,i3,j2) ...
                 -intlambda([i1,j1,i3,j2],3)*DiDj(:,i2,j3) ...
                 +intlambda([i1,j1,i3,j3],3)*DiDj(:,i2,j2);
        end
        Aij = Aij.*volume;
        Mij = Mij.*volume;
        sA(index+1:index+NT) = Aij;
        sM(index+1:index+NT) = Mij;
        index = index + NT;
    end
end
clear curlBasis_i curlBasis_j basis_i basis_j
diagIdx = (ii == jj);   upperIdx = ~diagIdx;
A = sparse(ii(diagIdx),jj(diagIdx),sA(diagIdx),Ndof,Ndof);
AU = sparse(ii(upperIdx),jj(upperIdx),sA(upperIdx),Ndof,Ndof);
A = A + AU + AU';
M = sparse(ii(diagIdx),jj(diagIdx),sM(diagIdx),Ndof,Ndof);
MU = sparse(ii(upperIdx),jj(upperIdx),sM(upperIdx),Ndof,Ndof);
M = M + MU + MU';
clear AU MU

% Whole matrix
bigA = [-M A; A M];

%% Righthand side
f = zeros(Ndof,1);
if ~isfield(pde,'f') || (isfield(pde,'f') && isreal(pde.f) && all(pde.f==0))
    pde.f = [];
end
if ~isfield(option,'fquadorder')
    option.fquadorder = 4;   % default order is 4
end
if isfield(pde,'f') && ~isempty(pde.f)
    [lambda,w] = quadpts3(option.fquadorder); % quadrature order is 4
    nQuad = size(lambda,1);
    bt = zeros(NT,20);
    for p = 1:nQuad
        % quadrature points in the x-y-z coordinate
        pxyz = lambda(p,1)*node(elem(:,1),:) ...
             + lambda(p,2)*node(elem(:,2),:) ...
             + lambda(p,3)*node(elem(:,3),:) ...
             + lambda(p,4)*node(elem(:,4),:);
        fp = pde.f(pxyz);
        for k = 1:20
            k1 = locBasesIdx(k,1);
            k2 = locBasesIdx(k,2);
            k3 = locBasesIdx(k,3);
            % evaluate basis at quadrature points
            if k<=6
            % phi_k = lambda_{k1}Dlambda_{k2} - lambda_{k2}Dlambda_{k1};
                basis_k = (lambda(p,k1)*Dlambda(:,:,k2) ...
                          -lambda(p,k2)*Dlambda(:,:,k1));
            elseif k<=12
            % psi_k = lambda_{k1}Dlambda_{k2} + lambda_{k2}Dlambda_{k1};
                basis_k = (lambda(p,k1)*Dlambda(:,:,k2) ...
                          +lambda(p,k2)*Dlambda(:,:,k1));
            else
            % chi_k = lambda_{k1}phi_{k2,k3};
                basis_k = lambda(p,k1)*(lambda(p,k2)*Dlambda(:,:,k3) ...
                                       -lambda(p,k3)*Dlambda(:,:,k2));
            end
            rhs = dot(basis_k,fp,2);
            bt(:,k) = bt(:,k) + w(p)*rhs;
        end
    end
    bt = bt.*repmat(volume,1,20);
    f = accumarray(elem2dof(:),bt(:),[Ndof 1]);
end
clear pxy fp bt rhs basis_k

bigf = [zeros(Ndof,1); f];
assembleTime = toc;

%% Boundary condition
% Find Dirichlet boundary dof: fixedDof
if isempty(bdFlag) && ~isempty(pde.g_D) && isempty(pde.g_N)
    % Dirichlet boundary condition only
    bdFlag = setboundary3(node,elem,'Dirichlet');
end
isBdDof = false(Ndof,1);

if ~isempty(bdFlag)
    %% Dirichlet boundary condition on edge dofs
    % Find boundary faces, edges and nodes
    isBdFace = false(NF,1);
    isBdFace(elem2face(bdFlag(:,1) == 1,1)) = true;
    isBdFace(elem2face(bdFlag(:,2) == 1,2)) = true;
    isBdFace(elem2face(bdFlag(:,3) == 1,3)) = true;
    isBdFace(elem2face(bdFlag(:,4) == 1,4)) = true;
%     boundaryFace = face(isBdFace,:);
    bdFace2edge = face2edge(isBdFace,:);
    isBdEdge = false(NE,1);
    isBdEdge(bdFace2edge(:)) = true;
    edgeBdDof = [find(isBdEdge); NE + find(isBdEdge)];
%     bdEdge = edge(isBdEdge,:);
%     isBdNode(bdEdge) = true;
%     bdNode = find(isBdNode);
    faceBdDof = 2*NE + [find(isBdFace); NF+find(isBdFace)];
%     edgeIdxMap = zeros(NE,1);
%     edgeIdxMap(isBdEdge) = 1:size(bdEdge,1);
%     bdFace2edge = edgeIdxMap(bdFace2edge);
    isBdDof(edgeBdDof) = true;
    isBdDof(faceBdDof) = true;
end

freeDof = ~isBdDof;

if any(freeDof)
    idx = [true(Ndof,1); freeDof];
    bigA_bd= bigA(idx,idx);
    bigf_bd = bigf(idx);
end

%% Solve
t = cputime;
bigu = bigA_bd\bigf_bd;
omega = bigu(1:Ndof);
u = zeros(Ndof,1);
u(freeDof) = bigu(Ndof+1:end);
info.solverTime = cputime -t;
display(info.solverTime);

%% Output information
eqn = struct('elem2edge',elem2edge,'elem2face',elem2face,'face2edge',face2edge,'A',A,'edge',edge,'face',face,'M',M,'f',f);
info.assembleTime = assembleTime;
end


clear all
        % Desemnarea variabilei x ca si simbolica
x=sym('x');
		% Incarcarea functiei de integrat
F=1/x
        % Calcularea integralei nedefinite
I1=int(F)
        % Calcularea integralei definite intre limitele 1 si 2
I2=int(F,1,2)


% RES = reconWpyr(PYR, INDICES, FILT, EDGES, LEVS, BANDS)
%
% Reconstruct image from its separable orthonormal QMF/wavelet pyramid
% representation, as created by buildWpyr.
%
% PYR is a vector containing the N pyramid subbands, ordered from fine
% to coarse.  INDICES is an Nx2 matrix containing the sizes of
% each subband.  This is compatible with the MatLab Wavelet toolbox.
%
% FILT (optional) can be a string naming a standard filter (see
% namedFilter), or a vector which will be used for (separable)
% convolution.  Default = 'qmf9'.  EDGES specifies edge-handling,
% and defaults to 'reflect1' (see corrDn).
%
% LEVS (optional) should be a vector of levels to include, or the string
% 'all' (default).  1 corresponds to the finest scale.  The lowpass band
% corresponds to wpyrHt(INDICES)+1.
%
% BANDS (optional) should be a vector of bands to include, or the string
% 'all' (default).   1=horizontal, 2=vertical, 3=diagonal.  This is only used
% for pyramids of 2D images.

% Eero Simoncelli, 6/96.

function res = reconWpyr(pyr, ind, filt, edges, levs, bands)

if (nargin < 2)
  error('First two arguments (PYR INDICES) are required');
end

%%------------------------------------------------------------
%% OPTIONAL ARGS:

if (exist('filt') ~= 1)
  filt = 'qmf9';
end

if (exist('edges') ~= 1)
  edges= 'reflect1';
end

if (exist('levs') ~= 1)
  levs = 'all';
end

if (exist('bands') ~= 1)
  bands = 'all';
end

%%------------------------------------------------------------

maxLev = 1+wpyrHt(ind);
if strcmp(levs,'all')
  levs = [1:maxLev]';
else
  if (any(levs > maxLev))
    error(sprintf('Level numbers must be in the range [1, %d].', maxLev));
  end
  levs = levs(:);
end

if strcmp(bands,'all')
  bands = [1:3]';
else
  if (any(bands < 1) | any(bands > 3))
    error('Band numbers must be in the range [1,3].');
  end
  bands = bands(:);
end

if isstr(filt)
  filt = namedFilter(filt);
end

filt = filt(:);
hfilt = modulateFlip(filt);

%% For odd-length filters, stagger the sampling lattices:
if (mod(size(filt,1),2) == 0)
	stag = 2;
else
	stag = 1;
end

%% Compute size of result image: assumes critical sampling (boundaries correct)
res_sz = ind(1,:);
if (res_sz(1) == 1)
  loind = 2;
  res_sz(2) = sum(ind(:,2));
elseif (res_sz(2) == 1)
  loind = 2;
  res_sz(1) = sum(ind(:,1));
else
  loind = 4;
  res_sz = ind(1,:) + ind(2,:);  %%horizontal + vertical bands.
  hres_sz = [ind(1,1), res_sz(2)];
  lres_sz = [ind(2,1), res_sz(2)];
end

%% First, recursively collapse coarser scales:
if any(levs > 1)

  if (size(ind,1) > loind)
    nres = reconWpyr( pyr(1+sum(prod(ind(1:loind-1,:)')):size(pyr,1)), ...
	ind(loind:size(ind,1),:), filt, edges, levs-1, bands);
  else
    nres = pyrBand(pyr, ind, loind); 	% lowpass subband
  end

  if (res_sz(1) == 1)
    res = upConv(nres, filt', edges, [1 2], [1 stag], res_sz);
  elseif (res_sz(2) == 1)
    res = upConv(nres, filt, edges, [2 1], [stag 1], res_sz);
  else
    ires = upConv(nres, filt', edges, [1 2], [1 stag], lres_sz);
    res = upConv(ires, filt, edges, [2 1], [stag 1], res_sz);
  end

else

  res = zeros(res_sz);

end

%% Add  in reconstructed bands from this level:
if any(levs == 1)
  if (res_sz(1) == 1)
    upConv(pyrBand(pyr,ind,1), hfilt', edges, [1 2], [1 2], res_sz, res);
  elseif (res_sz(2) == 1)
    upConv(pyrBand(pyr,ind,1), hfilt, edges, [2 1], [2 1], res_sz, res);
  else
    if any(bands == 1) % horizontal
      ires = upConv(pyrBand(pyr,ind,1),filt',edges,[1 2],[1 stag],hres_sz);
      upConv(ires,hfilt,edges,[2 1],[2 1],res_sz,res);  %destructively modify res
    end
    if any(bands == 2) % vertical
      ires = upConv(pyrBand(pyr,ind,2),hfilt',edges,[1 2],[1 2],lres_sz);
      upConv(ires,filt,edges,[2 1],[stag 1],res_sz,res);  %destructively modify res
    end
    if any(bands == 3) % diagonal
      ires =  upConv(pyrBand(pyr,ind,3),hfilt',edges,[1 2],[1 2],hres_sz);
      upConv(ires,hfilt,edges,[2 1],[2 1],res_sz,res);  %destructively modify res
    end
  end
end


function tapas_ehgf_jget_plotTraj(r)
% Plots the estimated trajectories for the HGF perceptual model for
% the JGET project
% Usage example:  est = tapas_fitModel(responses, inputs); tapas_ehgf_plotTraj(est);
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013-2020 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Optional plotting of standard deviations (true or false)
plotsd = true;

% Set up display
scrsz = get(0,'screenSize');
outerpos = [0.2*scrsz(3),0.2*scrsz(4),0.8*scrsz(3),0.8*scrsz(4)];
figure(...
    'OuterPosition', outerpos,...
    'Name', 'HGF trajectories');

% Time axis
if size(r.u,2) > 1 && ~isempty(find(strcmp(fieldnames(r.c_prc),'irregular_intervals'))) && r.c_prc.irregular_intervals
    t = r.u(:,end)';
else
    t = ones(1,size(r.u,1));
end

ts = cumsum(t);
ts = [0, ts];

% Do we know the generative parameters?
if size(r.u,2) > 2
    genpar = true;
    mean   = r.u(:,2);
    sd     = r.u(:,3);
else
    genpar = false;
end

% Number of levels
try
    l = r.c_prc.n_levels;
catch
    l = length(r.p_prc.p)/8;
end

% Upper levels
for j = 1:l-1

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Left subplot (x)                       %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    subplot(l+1,2,2*j-1);

    if plotsd == true
        upperprior = r.p_prc.mux_0(l-j+1) +1.96*sqrt(r.p_prc.sax_0(l-j+1));
        lowerprior = r.p_prc.mux_0(l-j+1) -1.96*sqrt(r.p_prc.sax_0(l-j+1));
        upper = [upperprior; r.traj.mux(:,l-j+1)+1.96*sqrt(r.traj.sax(:,l-j+1))];
        lower = [lowerprior; r.traj.mux(:,l-j+1)-1.96*sqrt(r.traj.sax(:,l-j+1))];

        plot(0, upperprior, 'ob', 'LineWidth', 1);
        hold all;
        plot(0, lowerprior, 'ob', 'LineWidth', 1);
        fill([ts, fliplr(ts)], [(upper)', fliplr((lower)')], ...
             'b', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
    end
    plot(ts, [r.p_prc.mux_0(l-j+1); r.traj.mux(:,l-j+1)], 'b', 'LineWidth', 1.5);
    hold all;
    plot(0, r.p_prc.mux_0(l-j+1), 'ob', 'LineWidth', 1.5); % prior
    xlim([0 ts(end)]);
    title(['Posterior expectation of x_' num2str(l-j+1)], 'FontWeight', 'bold');
    ylabel(['\mu x_', num2str(l-j+1)]);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Right subplot (alpha)                  %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    subplot(l+1,2,2*j);

    if plotsd == true
        upperprior = r.p_prc.mua_0(l-j+1) +1.96*sqrt(r.p_prc.saa_0(l-j+1));
        lowerprior = r.p_prc.mua_0(l-j+1) -1.96*sqrt(r.p_prc.saa_0(l-j+1));
        upper = [upperprior; r.traj.mua(:,l-j+1)+1.96*sqrt(r.traj.saa(:,l-j+1))];
        lower = [lowerprior; r.traj.mua(:,l-j+1)-1.96*sqrt(r.traj.saa(:,l-j+1))];

        plot(0, upperprior, 'ob', 'LineWidth', 1);
        hold all;
        plot(0, lowerprior, 'ob', 'LineWidth', 1);
        fill([ts, fliplr(ts)], [(upper)', fliplr((lower)')], ...
             'b', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
    end
    plot(ts, [r.p_prc.mua_0(l-j+1); r.traj.mua(:,l-j+1)], 'b', 'LineWidth', 1.5);
    hold all;
    plot(0, r.p_prc.mua_0(l-j+1), 'ob', 'LineWidth', 1.5); % prior
    xlim([0 ts(end)]);
    title(['Posterior expectation of \alpha_' num2str(l-j+1)], 'FontWeight', 'bold');
    ylabel(['\mu \alpha_', num2str(l-j+1)]);
end

% Input level
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Left subplot (x)                       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(l+1,2,2*l-1);

if plotsd == true
    upperprior = r.p_prc.mux_0(1) +1.96*sqrt(r.p_prc.sax_0(1));
    lowerprior = r.p_prc.mux_0(1) -1.96*sqrt(r.p_prc.sax_0(1));
    upper = [upperprior; r.traj.mux(:,1)+1.96*sqrt(r.traj.sax(:,1))];
    lower = [lowerprior; r.traj.mux(:,1)-1.96*sqrt(r.traj.sax(:,1))];

    plot(0, upperprior, 'or', 'LineWidth', 1);
    hold all;
    plot(0, lowerprior, 'or', 'LineWidth', 1);
    fill([ts, fliplr(ts)], [(upper)', fliplr((lower)')], ...
         'r', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
end
plot(ts, [r.p_prc.mux_0(1); r.traj.mux(:,1)], 'r', 'LineWidth', 1.5);
hold all;
plot(0, r.p_prc.mux_0(1), 'or', 'LineWidth', 1.5); % prior
plot(ts(2:end), r.u(:,1), '.', 'Color', [0 0.6 0]); % inputs
if genpar
    plot(ts(2:end), mean, '-', 'Color', 'k', 'LineWidth', 1); % mean of input distribution
    plot(ts(2:end), mean +1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1); % 95% interval of input distribution
    plot(ts(2:end), mean -1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1); % 95% interval of input distribution
end
xlim([0 ts(end)]);
title(['Input u (green) and posterior expectation of x_1 (red) for \kappa_x=', ...
       num2str(r.p_prc.kax), ', \omega_x=', num2str(r.p_prc.omx)], 'FontWeight', 'bold');
ylabel('u, \mu x_1');
hold off;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Right subplot (alpha)                  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(l+1,2,2*l);

if plotsd == true
    upperprior = r.p_prc.mua_0(1) +1.96*sqrt(r.p_prc.saa_0(1));
    lowerprior = r.p_prc.mua_0(1) -1.96*sqrt(r.p_prc.saa_0(1));
    upper = [upperprior; r.traj.mua(:,1)+1.96*sqrt(r.traj.saa(:,1))];
    lower = [lowerprior; r.traj.mua(:,1)-1.96*sqrt(r.traj.saa(:,1))];

    transupperprior = sqrt(exp(r.p_prc.kau *upperprior +r.p_prc.omu));
    translowerprior = sqrt(exp(r.p_prc.kau *lowerprior +r.p_prc.omu));
    transupper = sqrt(exp(r.p_prc.kau *upper +r.p_prc.omu));
    translower = sqrt(exp(r.p_prc.kau *lower +r.p_prc.omu));

    plot(0, transupperprior, 'or', 'LineWidth', 1);
    hold all;
    plot(0, translowerprior, 'or', 'LineWidth', 1);
    fill([ts, fliplr(ts)], [(transupper)', fliplr((translower)')], ...
         'r', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
end
transmuaprior = sqrt(exp(r.p_prc.kau *r.p_prc.mua_0(1) +r.p_prc.omu));
plot(ts, [transmuaprior; sqrt(exp(r.p_prc.kau *r.traj.mua(:,1) +r.p_prc.omu))], 'r', 'LineWidth', 1.5);
hold all;
plot(0, transmuaprior, 'or', 'LineWidth', 1.5); % prior
if genpar
    plot(ts(2:end), sd, '--', 'Color', 'k', 'LineWidth', 1);
end
xlim([0 ts(end)]);
title(['Belief on noise (red) for \kappa_\alpha=', ...
       num2str(r.p_prc.kaa), ', \omega_\alpha=', num2str(r.p_prc.oma)], 'FontWeight', 'bold');
ylabel('\mu \alpha_1');
hold off;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Decision model                         %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(l+1,2,2*l+1);

if plotsd == true
    upper = r.traj.muxhat(:,1)+1.96*sqrt(r.p_obs.ze +r.traj.saxhat(:,1));
    lower = r.traj.muxhat(:,1)-1.96*sqrt(r.p_obs.ze +r.traj.saxhat(:,1));

    fill([ts(2:end), fliplr(ts(2:end))], [(upper)', fliplr((lower)')], ...
         'r', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
    hold all;
end
plot(ts(2:end), r.traj.muxhat(:,1), 'Color', [153/256 17/256 153/256], 'LineWidth', 1.5);
hold all;
plot(ts(2:end), r.y(:,1), '.', 'Color', [1 0.65 0], 'MarkerSize', 15); % responses
if genpar
    plot(ts(2:end), mean, '-', 'Color', 'k', 'LineWidth', 1); % mean of input distribution
    plot(ts(2:end), mean +1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1); % 95% interval of input distribution
    plot(ts(2:end), mean -1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1); % 95% interval of input distribution
end
xlim([1 ts(end)]);
title('Decision model: prediction of decision (purple) and decision (orange)', 'FontWeight', 'bold');
ylabel('y, \^{\mu} x_1');
xlabel({'Trial number', ' '}); % A hack to get the relative subplot sizes right
hold off;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Learning rate                         %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if genpar
subplot(l+1,2,2*l+2);
[AX, H1, H2 ] = plotyy(ts(2:end), mean, ts(2:end), r.traj.lrx(:,1));
hold all;
ylim(AX(1), [min(mean -1.96.*sd-3), max(mean +1.96.*sd+3)]);
plot(AX(1), ts(2:end), mean +1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1.1); % 95% interval of input distribution
plot(AX(1), ts(2:end), mean -1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1.1); % 95% interval of input distribution
set(H1, 'Color', 'k', 'LineWidth', 1.1);
set(H2, 'Color', [178/256, 34/256, 34/256], 'LineWidth', 1.5);
set(AX(1), 'YColor', 'k');
set(AX(2), 'YColor', 'k');
xlim(AX(1), [1 ts(end)]);
xlim(AX(2), [1 ts(end)]);
title('Learning rate (bordeaux) and input sampling distribution (black)', 'FontWeight', 'bold');
ylabel(AX(1), 'Input');
ylabel(AX(2), '\^{\pi}_u/\pi_x');
xlabel('Trial number');
hold off;
end


function [ abd, rcond, z, info ] = spbco ( abd, lda, n, m )

%*****************************************************************************80
%
%% SPBCO factors a real symmetric positive definite banded matrix.
%
%  Discussion:
%
%    SPBCO also estimates the condition of the matrix.
%
%    If RCOND is not needed, SPBFA is slightly faster.
%
%    To solve A*X = B, follow SPBCO by SPBSL.
%
%    To compute inverse(A)*C, follow SPBCO by SPBSL.
%
%    To compute determinant(A), follow SPBCO by SPBDI.
%
%  Band storage:
%
%    If A is a symmetric positive definite band matrix, the following
%    program segment will set up the input.
%
%      m = (band width above diagonal)
%      do j = 1, n
%        i1 = max (1, j-m)
%        do i = i1, j
%          k = i-j+m+1
%          abd(k,j) = a(i,j)
%        end do
%      end do
%
%    This uses M + 1 rows of A, except for the M by M upper left triangle,
%    which is ignored.
%
%    For example, if the original matrix is
%
%      11 12 13  0  0  0
%      12 22 23 24  0  0
%      13 23 33 34 35  0
%       0 24 34 44 45 46
%       0  0 35 45 55 56
%       0  0  0 46 56 66
%
%    then N = 6, M = 2  and ABD should contain
%
%       *  * 13 24 35 46
%       * 12 23 34 45 56
%      11 22 33 44 55 66
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 June 2009
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real ABD(LDA,N), the matrix to be factored.  The columns of the
%    upper triangle are stored in the columns of ABD and the diagonals of
%    the upper triangle are stored in the rows of ABD.
%
%    Input, integer LDA, the leading dimension of the array ABD.
%    M+1 <= LDA is required.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer M, the number of diagonals above the main diagonal.
%
%    Output, real ABD(LDA,N), an upper triangular matrix R, stored in band form,
%    so that A = R'*R.  If INFO /= 0, the factorization is not complete.
%
%    Output, real RCOND, an estimate of the reciprocal condition
%    of A.  For the system A*X = B, relative perturbations in A and B of size
%    EPSILON may cause relative perturbations in X of size EPSILON/RCOND.
%    If RCOND is so small that the logical expression
%      1.0 + RCOND == 1.0
%    is true, then A may be singular to working precision.  In particular,
%    RCOND is zero if exact singularity is detected or the estimate underflows.
%
%    Output, real Z(N), a work vector whose contents are usually
%    unimportant.  If A is singular to working precision, then Z is an
%    approximate null vector in the sense that
%      norm(A*Z) = RCOND * norm(A) * norm(Z).
%    If INFO /= 0, Z is unchanged.
%
%    Output, integer INFO, error flag.
%    0, for normal return.
%    K, signals an error condition.  The leading minor of order K is not
%    positive definite.
%

%
%  Find the norm of A.
%
  for j = 1 : n

    l = min ( j, m+1 );
    mu = max ( m+2-j, 1 );
    z(j) = sasum ( l, abd(mu:mu+l-1,j), 1 );
    k = j - l;
    for i = mu : m
      k = k + 1;
      z(k) = z(k) + abs ( abd(i,j) );
    end

  end

  anorm = max ( z(1:n) );
%
%  Factor.
%
  [ abd, info ] = spbfa ( abd, lda, n, m );

  if ( info ~= 0 )
    return
  end
%
%  RCOND = 1/(norm(A)*(estimate of norm(inverse(A)))).
%
%  Estimate = norm(Z)/norm(Y) where A*Z = Y and A*Y = E.
%
%  The components of E are chosen to cause maximum local
%  growth in the elements of W where R'*W = E.
%
%  The vectors are frequently rescaled to avoid overflow.
%
%  Solve R' * W = E.
%
  ek = 1.0;
  z(1:n) = 0.0;

  for k = 1 : n

    if ( z(k) ~= 0.0 )
      ek = - abs ( ek ) * r4_sign ( z(k) );
    end

    if ( abd(m+1,k) < abs ( ek - z(k) ) )
      s = abd(m+1,k) / abs ( ek - z(k) );
      z(1:n) = s * z(1:n);
      ek = s * ek;
    end

    wk = ek - z(k);
    wkm = -ek - z(k);
    s = abs ( wk );
    sm = abs ( wkm );
    wk = wk / abd(m+1,k);
    wkm = wkm / abd(m+1,k);
    j2 = min ( k+m, n );
    i = m + 1;

    if ( k+1 <= j2 )

      for j = k+1 : j2
        i = i - 1;
        sm = sm + abs ( z(j) + wkm * abd(i,j) );
        z(j) = z(j) + wk * abd(i,j);
        s = s + abs ( z(j) );
      end

      if ( s < sm )

        t = wkm - wk;
        wk = wkm;
        i = m + 1;

        for j = k+1 : j2
          i = i - 1;
          z(j) = z(j) + t * abd(i,j);
        end

      end

    end

    z(k) = wk;

  end

  z(1:n) = z(1:n) / sasum ( n, z(1:n), 1 );
%
%  Solve R * Y = W.
%
  for k = n : -1 : 1

    if ( abd(m+1,k) < abs ( z(k) ) )
      s = abd(m+1,k) / abs ( z(k) );
      z(1:n) = s * z(1:n);
    end

    z(k) = z(k) / abd(m+1,k);
    lm = min ( k-1, m );
    la = m + 1 - lm;
    lb = k - lm;
    t = -z(k);
    z(lb:lb+lm-1) = z(lb:lb+lm-1) + t * abd(la:la+lm-1,k)';

  end

  z(1:n) = z(1:n) / sasum ( n, z(1:n), 1 );

  ynorm = 1.0;
%
%  Solve R' * V = Y.
%
  for k = 1 : n

    lm = min ( k-1, m );
    la = m + 1 - lm;
    lb = k - lm;

    z(k) = z(k) - sdot ( lm, abd(la:la+lm-1,k), 1, z(lb:lb+lm-1), 1 );

    if ( abd(m+1,k) < abs ( z(k) ) )
      s = abd(m+1,k) / abs ( z(k) );
      z(1:n) = s * z(1:n);
      ynorm = s * ynorm;
    end

    z(k) = z(k) / abd(m+1,k);

  end

  s = 1.0 / sasum ( n, z(1:n), 1 );
  z(1:n) = s * z(1:n);
  ynorm = s * ynorm;
%
%  Solve R * Z = W.
%
  for k = n : -1 : 1

    if ( abd(m+1,k) < abs ( z(k) ) )
      s = abd(m+1,k) / abs ( z(k) );
      z(1:n) = s * z(1:n);
      ynorm = s * ynorm;
    end

    z(k) = z(k) / abd(m+1,k);
    lm = min ( k-1, m );
    la = m + 1 - lm;
    lb = k - lm;
    t = -z(k);
    z(lb:lb+lm-1) = z(lb:lb+lm-1) + t * abd(la:la+lm-1,k)';

  end
%
%  Make ZNORM = 1.0.
%
  s = 1.0 / sasum ( n, z(1:n), 1 );
  z(1:n) = s * z(1:n);
  ynorm = s * ynorm;

  if ( anorm ~= 0.0 )
    rcond = ynorm / anorm;
  else
    rcond = 0.0;
  end

  return
end


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


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


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


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


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


%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