function c = vertcat(varargin)
% function C=vertcat(A,B);
%
% DESCRIPTION
%   Vertical concatenation of polynomial objects.
%
% INPUTS
%   A,B: polynomials
%
% OUTPUTS
%   C:  vertical concatenation of input matrices.
%
% SYNTAX
%   [A; B]
%      Vertical concatenation of polynomial matrices A and B.
%      A and B must have the same number of columns.
%   [A1; A2; A3; ...]
%      Vertical concatenation of several polynomial matrices.
%   C = vertcat(A1,A2,A3,..);
%      Function-call form of vertical concatenation.
%
% See also horzcat

% 6/8/2002: PJS  Initial Coding

if nargin==1
    c = varargin{1};
else
    % Promote a to polynomial
    a = polynomial(varargin{1});
    [nra,nca] = size(a);

    % Promote b to polynomial
    b = polynomial(varargin{2});
    [nrb,ncb] = size(b);

    if isempty(b);
        c = a;
    elseif isempty(a);
        c = b;
    elseif nca == ncb
        % Get Dimensions
        nta = size(a.degmat,1);
        nva = length(a.varname);
        ntb = size(b.degmat,1);
        nvb = length(b.varname);

        if nva==0 && nvb==0
            % Combine constant terms
            ar = combine(a);
            coef1 = reshape(ar.coefficient,[nra nca]);
            br = combine(b);
            coef2 = reshape(br.coefficient,[nrb ncb]);

            % Stack up Coefficients and Form Polynomial
            coefficient = [coef1; coef2];
            c = polynomial(coefficient);
        else
            % Form Degmat, Varname, and Matdim
            adeg = a.degmat;
            bdeg = b.degmat;
            degmat = blkdiag(adeg,bdeg);
            varname = [a.varname(:); b.varname(:)];
            matdim = [nra+nrb nca];

            % Stack up Coefficients
            idx1 = [];
            idx2 = [];
            for i1 = 0:(nca-1);
                idx1 = [idx1 (1:nra)+i1*(nra+nrb)];
                idx2 = [idx2 nra+(1:nrb)+i1*(nra+nrb)];
            end;
            coef1 = spalloc(nta,(nra+nrb)*nca,nnz(a.coefficient));
            coef1(:,idx1) = a.coefficient;
            coef2 = spalloc(ntb,(nra+nrb)*ncb,nnz(b.coefficient));
            coef2(:,idx2) = b.coefficient;
            coefficient = [coef1; coef2];

            % Form Polynomial and combine terms
            chkval = 0; % skip validity check
            c = polynomial(coefficient,degmat,varname,matdim,chkval);
            c = combine(c);
        end

    else
        error('All columns must have the same row dimension')
    end

    if nargin>2
        c = vertcat(c,varargin{3:end});
    end
end


function[varargout]=makefigs_gulftransfer(str)
%MAKEFIGS_TRANSFER Makes all figures for Lilly and Elipot (2021).
%
%   This function makes all figures for
%
%        Lilly, J. M. and S. Elipot (2021). A unifying perspective on
%            transfer function solutions to the unsteady Ekman problem.
%            Fluids, 6 (2): 85, 1--36.
%
%   This may take a while as some of the plots are computationally
%   intensive.
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2021 J.M. Lilly --- type 'help jlab_license' for details

if nargin==0
  str='noprint';
end

if strcmp(str,'--f')
     makefigs_gulfdrifters('noprint');return
end

%This is where you want things to be printed to
dirname='/Users/lilly/Desktop/Dropbox/Projects/transfer/theory/figures';

%/*************************************************************************
%basic figure of transfer function
rho=1027;
fc=1e-4;
omega=[-8.5:.01:8.5]*fc;
z=[0:.1:50]';
zo=20;
delta=20;zo=20;h=50;mu=delta.^2./zo;
G1=rho.*fc.*windtrans(omega*24*3600,z,fc*24*3600,delta,mu,inf);
G2=rho.*fc.*windtrans(omega*24*3600,z,fc*24*3600,delta,mu,50);
G1(~isfinite(G1))=1e6;
[ommat,zmat]=meshgrid(omega,z);
zeta=2*sqrt(2)*frac(zo,delta).*sqrt((1+zmat./zo).*abs(1+ommat./fc));

%str='Transfer Function Magnitude (m^2s/kg)';
str='Log10 Transfer Function Magnitude (m^{-1})';

figure,
clf
subplot(1,2,1),contourf(omega./fc,z,log10(abs(G1))',-10:.1:6),nocontours,flipy,hold on
text(-8,48,'(a)','color','w')
subplot(1,2,2),contourf(omega./fc,z,log10(abs(G2))',-10:.1:6),nocontours,flipy,hold on
text(-8,48,'(b)','color','w')

for i=1:2
    subplot(1,2,i)
    caxis([-4 -1]),hlines(20,'0.5k:'),%vlines(-1,'w'),
    %hlines([10 40],'2w')
    if i==1
        hlines([10 40],'0.5D')
    else
        hlines([40],'0.5D')
    end
    xtick([-10:2:10])
    ylabel('Depth (m)')
    xlabel('Nondimensional Frequency $\omega/|f|$','interpreter','latex')
    %contour(omega./fc,z,zeta,[4 7],'w','linewidth',2)
    contour(omega./fc,z,zeta,[4 7.5],'w')
end
subplot(1,2,1)
text(-1,5,'I','color',[1 1 1],'HorizontalAlignment','center')
text(-1,25,'IV','color',[1 1 1],'HorizontalAlignment','center')
text(-1,45,'VII','color',[1 1 1],'HorizontalAlignment','center')
text(2,5,'II','color',[1 1 1]*0,'HorizontalAlignment','center')
text(-4,5,'II','color',[1 1 1]*0,'HorizontalAlignment','center')
text(1,25,'V','color',[1 1 1]*0,'HorizontalAlignment','center')
text(-3,25,'V','color',[1 1 1]*0,'HorizontalAlignment','center')
text(0.4,45,'VIII','color',[1 1 1]*0,'HorizontalAlignment','center')
text(-2.4,45,'VIII','color',[1 1 1]*0,'HorizontalAlignment','center')
text(5.5,5,'III','color',[1 1 1]*0,'HorizontalAlignment','center')
text(-7.5,5,'III','color',[1 1 1]*0,'HorizontalAlignment','center')
text(5.5,25,'VI','color',[1 1 1]*0,'HorizontalAlignment','center')
text(-7.5,25,'VI','color',[1 1 1]*0,'HorizontalAlignment','center')
text(5.5,45,'IX','color',[1 1 1],'HorizontalAlignment','center')
text(-7.5,45,'IX','color',[1 1 1],'HorizontalAlignment','center')

subplot(1,2,2)
%text(-1,5,'I-$h$','color',[1 1 1],'HorizontalAlignment','center')
text(-1,25,'IV-$h$','color',[1 1 1],'HorizontalAlignment','center')
text(-1,45,'VII-$h$','color',[1 1 1]*0,'HorizontalAlignment','center')
%text(2,5,'II-$h$','color',[1 1 1]*0,'HorizontalAlignment','center')
%text(-4,5,'II-$h$','color',[1 1 1]*0,'HorizontalAlignment','center')
text(1,25,'V-$h$','color',[1 1 1]*0,'HorizontalAlignment','center')
text(-3,25,'V-$h$','color',[1 1 1]*0,'HorizontalAlignment','center')
text(0.4,45,'VIII-$h$','color',[1 1 1]*0,'HorizontalAlignment','center')
text(-2.4,45,'VIII-$h$','color',[1 1 1]*0,'HorizontalAlignment','center')
%text(5.5,5,'III-$h$','color',[1 1 1]*0,'HorizontalAlignment','center')
%text(-7.5,5,'III-$h$','color',[1 1 1]*0,'HorizontalAlignment','center')
text(5.5,25,'VI-$h$','color',[1 1 1]*0,'HorizontalAlignment','center')
text(-7.5,25,'VI-$h$','color',[1 1 1]*0,'HorizontalAlignment','center')
text(5.5,45,'IX-$h$','color',[1 1 1],'HorizontalAlignment','center')
text(-7.5,45,'IX-$h$','color',[1 1 1],'HorizontalAlignment','center')

ha=packfig(1,2,'col');
axes(ha(1)),%ylim([0 62]),ytick([0:10:50])
hc=colorbar('SouthOutside');
hc.Label.String=str;
pos1=get(ha(1),'position');
pos2=get(ha(2),'position');
pos=get(hc,'position');%pos(4)=pos(4)/2;
%set(hc,'position',[pos(1)+0.195 pos(2) pos(3) pos(4)])
set(hc,'position',[pos(1)+0.195 pos(2)+.02 pos(3) pos(4)/2])
set(ha(2),'position',[pos2(1) pos1(2) pos2(3) pos1(4)])
set(ha(1),'position',pos1)
fontsize 10 10 9 8
set(gcf,'paperposition',[1 1 10 6])
if strcmp(str,'print')
    jprint(dirname,'transferfunctionschematic')
end
% %--------------------------------------------------------------------------
% %Green's function
% rho=1027;
% fc=1e-4;
% %omega=[-100:.01:100]*fc;
% omega=[-1000:.01:1000]*fc;
% z=[0:.1:49.9]';
% zo=20;
% delta=20;zo=20;h=50;mu=delta.^2./zo;
% %G1=rho.*fc.*windtrans(omega*24*3600,z,fc*24*3600,delta,mu,inf);
% G2=rho.*fc.*windtrans(omega*24*3600,z,fc*24*3600,delta,mu,50);
% g1=ifft(ifftshift(G1,1));
% g2=ifft(ifftshift(G2,1));
%
% figure,
% jpcolor(1:1e4,z,log10(abs(g2(1:1e4,:)))'),caxis([-7 -3.5]),flipy,hold on
% contour(1:1e4,z,real(g2(1:1e4,:))',[0 0],'k')
%\*************************************************************************

%/*************************************************************************
%basic figure of transfer function, flower version
rho=1027;
fc=1e-4;
omega=[-8.5:.01:8.5]*fc;
z=[0:5:45]';
zo=20;
delta=20;zo=20;h=50;mu=delta.^2./zo;
lnomega=logspace(2,100,10000);
omega=[-fliplr(lnomega) -100:.1:-8.6 -8.5:.001:-1-0.005 -1-0.005:1e-7:-1+0.005  -1+0.005:0.001 :8.5 8.6:.1:100 lnomega]*fc;

G1=rho.*abs(fc).*windtrans(omega*24*3600,z,fc*24*3600,delta,mu,inf);
G2=rho.*abs(fc).*windtrans(omega*24*3600,z,fc*24*3600,delta,mu,50);
figure
clf,
subplot(1,2,1)
hl=plot(G1);hold on,linestyle(hl,'0.5D'),
hl=plot(G1(:,1)); linestyle(hl,'2k')
hl=plot(G1(:,end));linestyle(hl,'2k--')
subplot(1,2,2)
hl=plot(G2);hold on,linestyle(hl,'0.5D'),
hl=plot(G2(:,1)); linestyle(hl,'2k')
hl=plot(G2(:,end));linestyle(hl,'2k--')
for i=1:2
    subplot(1,2,i)
    axis square, axis equal
    if i==1
        axis([-.1 1.5 -.8 .8]),
        xtick([-0.8:.2:1.6])
        text(1.35,0.7,['(' setstr(96+i) ')'])
    elseif i==2
        axis([-.01 .13 -.07 .07])
        text(0.117,0.06,['(' setstr(96+i) ')'])
    end

    vlines(0,'0.5k:'),hlines(0,'0.5k:')
    xlabel('Real Part of $\widetilde G(\omega,z)$ (m$^{-1}$)','interpreter','latex')%(Dimensionless)')
    ylabel('Imaginary Part of $\widetilde G(\omega,z)$ (m$^{-1}$)','interpreter','latex')% (Dimensionless)')
    %xtick([-.4:.2:1.2]),ytick([-.6:.2:.6])
end
set(gcf,'paperposition',[1 1 10 5])
if strcmp(str,'print')
    jprint(dirname,'schematicflowers')
end
%\*************************************************************************

%/*************************************************************************
%green's functions for schematic
rho=1027;
fc=1e-4;
%omega=[-100:.0001:100]*fc;
omega=[-10000:.01:10000]*fc;
z=[0:1/2:49.5]';
zo=20;
delta=20;zo=20;h=50;mu=delta.^2./zo;

[G1,G2]=vzeros(length(omega),length(z));

%z=[0:5:45]';
parfor i=1:length(z)
    i
%   G1(:,i)=rho.*abs(fc).*windtrans(omega*24*3600,z(i),fc*24*3600,delta,mu,inf);
    G2(:,i)=rho.*abs(fc).*windtrans(omega*24*3600,z(i),fc*24*3600,delta,mu,50);
end
G2(:,end+1)=1e-16+0*G2(:,end);
z=[z;50];
%g1=ifft(ifftshift(G1,1));
g2=ifft(ifftshift(G2,1));
g2=g2./maxmax(abs(g2));

%fc=2 pi rad/ 10^4 seconds
%fN= 2 pi rad
%dt= 1/2 seconds
%Tf = 2 pi / fc = 10^4 seconds
t=[1:size(g2,1)]'*frac(1,2)./10^4;

figure
ii=1:40000;
subplot(3,1,1)
contourf(t(ii),z,log10(abs(g2(ii,:)))',[-10:.1:0]);nocontours,hold on,flipy
contour(t(ii),z,log10(abs(g2(ii,:)))',[-12:1:0],'w');
hlines(z(1:10:end),'0.2D')
hlines(z(1),'2k')
hlines(z(end-10),'2k--')
%contour(t(ii),z,real(g2(ii,:))',[0 0],'k:');
caxis([-5.5 -1.5]),ylim([-.01 49.999]),xlim([0 2]),xtick([0:.25:2])
vlines(0.25005+[0:1/2:2],'0.25k:')
ylabel('Depth (m)')
text(1.875,2.5,'(a)','color','k')
subplot(3,1,2)
h=plot(t(ii),real(g2(ii,1:10:end))*1000);ylim(1000*[-.009 .009]+1000*0.005)
linestyle(h,'0.5D'),linestyle(h(1),'2k'),linestyle(h(end-1),'2k--')
ylabel('Real Part of $g(t,z)$ ($\times 1000 / \mathrm{max}\{g\}$)','interpreter','latex')
text(1.875,12.7,'(b)','color','k')
hold on,vlines(0.25005+[0:1/2:2],'0.25k:'),xtick([0:.25:2])
%h=plot(t(ii),abs(g2(ii,1:10:end))*1000);
subplot(3,1,3)
h=plot(t(ii),imag(g2(ii,1:10:end))*1000);ylim(1000*[-.009 .009])
linestyle(h,'0.5D'),linestyle(h(1),'2k'),linestyle(h(end-1),'2k--')
ylabel('Imaginary Part of $g(t,z)$ ($\times 1000 / \mathrm{max}\{g\}$)','interpreter','latex')
xlabel('Time (Inertial Periods $2\pi / f$)','interpreter','latex')
text(1.875,8,'(c)','color','k')
hold on,vlines(0.25005+[0:1/2:2],'0.25k:'),xtick([0:.25:2])
packfig(3,1)

set(gcf,'paperposition',[1 1 5 10])
if strcmp(str,'print')
    jprint(dirname,'greensfunction')
end
%\*************************************************************************

if 0
%/*************************************************************************
%green's functions for schematic
rho=1027;
fc=1e-4;
%omega=[-100:.0001:100]*fc;
omega=[-10000:.01:10000]*fc;
z=[0:1/2:49.5]';
delta=20;zo=20;h=50;mu=delta.^2./zo;
delta=10;zo=10;h=50;mu=delta.^2./zo;

[G1,G2]=vzeros(length(omega),length(z));

%z=[0:5:45]';
parfor i=1:length(z)
    i
   % G1(:,i)=rho.*abs(fc).*windtrans(omega*24*3600,z(i),fc*24*3600,delta,mu,inf);
    G2(:,i)=rho.*abs(fc).*windtrans(omega*24*3600,z(i),fc*24*3600,delta,mu,50);
end
%G1(~isfinite(G1))=0;
G2(:,end+1)=1e-16+0*G2(:,end);
z=[z;50];
%g1=ifft(ifftshift(G1,1));
g2=ifft(ifftshift(G2,1));
g2=g2./maxmax(abs(g2));

%fc=2 pi rad/ 10^4 seconds
%fN= 2 pi rad
%dt= 1/2 seconds
%Tf = 2 pi / fc = 10^4 seconds
t=[1:size(g2,1)]'*frac(1,2)./10^4;

clf
ii=1:40000;
subplot(3,1,1)
contourf(t(ii),z,log10(abs(g2(ii,:)))',[-12:.1:0]);nocontours,hold on,flipy
contour(t(ii),z,log10(abs(g2(ii,:)))',[-12:1/2:0],'w');
hlines(z(1:10:end),'0.2D'),hlines(z(1),'2k'),hlines(z(end-10),'2k--')
%contour(t(ii),z,real(g2(ii,:))',[0 0],'k:');
caxis([-5.5 -1.5]),ylim([-.01 49.999]),xlim([0 2]),xtick([0:.25:2])
vlines(0.25005+[0:1/2:2],'0.25k:')
ylabel('Depth (m)')
text(1.875,2.5,'(a)','color','k')
subplot(3,1,2)
h=plot(t(ii),real(g2(ii,1:10:end))*1000);ylim(1000*[-.009 .009]+1000*0.005-1.5)
linestyle(h,'0.5D'),linestyle(h(1),'2k'),linestyle(h(end-1),'2k--')
ylabel('Real Part of $g(t,z)$ ($\times 10^3 / \mathrm{max}\{g\}$)','interpreter','latex')
text(1.875,12.7-1.5,'(b)','color','k')
hold on,vlines(0.25005+[0:1/2:2],'0.25k:'),xtick([0:.25:2])
%h=plot(t(ii),abs(g2(ii,1:10:end))*1000);
subplot(3,1,3)
h=plot(t(ii),imag(g2(ii,1:10:end))*1000);ylim(1000*[-.009 .009])
linestyle(h,'0.5D'),linestyle(h(1),'2k'),linestyle(h(end-1),'2k--')
ylabel('Imaginary Part of $g(t,z)$ ($\times 10^3 / \mathrm{max}\{g\}$)','interpreter','latex')
text(1.875,8,'(c)','color','k')
hold on,vlines(0.25005+[0:1/2:2],'0.25k:'),xtick([0:.25:2])
xlabel('Time (Inertial Periods $2\pi / f$)','interpreter','latex')
% subplot(4,1,4)
% %gke=5*fliplr(log10(cumsum(squared(fliplr(g2)),2)));
% gke=5*log10(cumsum(squared(g2),2));
% contourf(t(ii),z,gke(ii,:)',[-50:.5:0]);nocontours,hold on,flipy
% contour(t(ii),z,gke(ii,:)',[-50:5:0],'w');
% hlines(z(1:10:end),'0.2D'),hlines(z(1),'2k'),hlines(z(end-10),'2k--')
% caxis([-40 -5]),ylim([-.01 49.999]),xlim([0 2]),xtick([0:.25:2])
% vlines(0.25005+[0:1/2:2],'0.25k:')
% text(1.875,2.5,'(d)','color','k')
% %ylabel('Kinetic Energy $\int_{-h}^z |g(t,x)|^2 \mathrm{d}x$')
% ylabel('Kinetic Energy $\int_{0}^z |g(t,x)|^2 \mathrm{d}x$')
% xlabel('Time (Inertial Periods $2\pi / f$)','interpreter','latex')
% packfig(4,1)
%set(gcf,'paperposition',[1 1 5 13])

packfig(3,1)
set(gcf,'paperposition',[1 1 5 10])
if strcmp(str,'print')
    jprint(dirname,'greensfunction_broader')
end
%\*************************************************************************
end

%/*************************************************************************
%illustration of problems with overflow
log10delta=linspace(-1,4,100);
log10mu=linspace(-7,4,100);
h=100;
%h=1000;
%h=1e6;
%h=1e8;
fc=1e-4;

Gi1=vzeros(length(log10mu),length(log10delta));
Gi2=vzeros(length(log10mu),length(log10delta));
Gi3=vzeros(length(log10mu),length(log10delta));
Gi4=vzeros(length(log10mu),length(log10delta));
%f=0;
f=2.*fc*24*3600;
%f=fc*24*3600*0.99;
%f=fc*24*3600*8;
tic
for i=1:length(log10mu)
    i
    for j=1:length(log10delta)
     Gi1(i,j)=windtrans(f,15,fc*24*3600,10.^log10delta(j),10.^log10mu(i),h,'two');
     Gi2(i,j)=windtrans(f,15,fc*24*3600,10.^log10delta(j),10.^log10mu(i),h,'far');
     Gi3(i,j)=windtrans(f,15,fc*24*3600,10.^log10delta(j),10.^log10mu(i),h,'general');
     Gi4(i,j)=windtrans(f,15,fc*24*3600,10.^log10delta(j),10.^log10mu(i),h,'expansion');
    end
end
%etime1=toc
%etime2=toc

figure,
clf
subplot(1,3,1),contourf(log10delta,log10mu,log10(abs(Gi3)),[-200:.1:0]),nocontours,hold on
contour(log10delta,log10mu,log10(abs(Gi3)),[-200:10:0],'w'),caxis([-12 maxmax(log10(abs(Gi1)))])
hc=colorbar('South');
hc.Label.String='Log10 Magnitude at $\omega = -2f$';
hc.Label.Interpreter='latex';
pos=get(hc,'position');
set(hc,'position',[pos(1)+pos(1)/10 pos(2) pos(3)+pos(1)/10 pos(4)/2])
subplot(1,3,2),contourf(log10delta,log10mu,log10(abs(Gi1)),[-200:.1:0]),nocontours,hold on
contour(log10delta,log10mu,log10(abs(Gi1)),[-200:10:0],'w'),caxis([-12 maxmax(log10(abs(Gi1)))])
subplot(1,3,3),contourf(log10delta,log10mu,log10(abs(Gi2-Gi4)./abs(Gi4)),[-14:.1:0]),nocontours,hold on
caxis([-14 -3])
hc2=colorbar('North');
hc2.Label.String='Log10 Fractional Error at $\omega = -2f$';
hc2.Label.Interpreter='latex';
pos2=get(hc2,'position');
set(hc2,'position',[pos2(1)-pos2(1)/28 pos2(2)-0.03 pos2(3)+pos(1)/10 pos2(4)/2])

[demat,mumat]=meshgrid(10.^log10delta,10.^log10mu);
for i=1:3
    subplot(1,3,i),%contour(log10delta,log10mu,log10(abs(Gi1-Gi2)./abs(Gi1)),[-3.4 -3.4],'k','linewidth',2)
    text(3.5,3.7,['(' setstr(96+i) ')'],'color','k')
    xlim([-1 3.99])
    axis equal,xlim([-1 3.99]),ylim([-5 4])
    ylabel('Log10 Madsen Depth $\mu$ (m)','interpreter','latex')
    xlabel('Log10 Ekman Depth $\delta$ (m)','interpreter','latex')
    %contour(log10delta,log10mu,log10(demat./mumat*2*sqrt(2)*sqrt(3)),[1 1]*3,'color',[1 1 1]*0.6,'linewidth',2)
    contour(log10delta,log10mu,log10(demat./mumat*2*sqrt(2)*sqrt(3)),[1 1]*2.9,'color',[1 1 1]*0.6,'linewidth',2)
end
packfig(1,3,'columns')
set(hc,'position',[pos(1)+pos(1)/10 pos(2)+.1 pos(3)+pos(1)/10 pos(4)/2])
set(hc2,'position',[pos2(1)-pos2(1)/28 pos2(2)-0.03-.07 pos2(3)+pos(1)/10 pos2(4)/2])
%set(gcf,'paperposition',[1 1 11 5])
set(gcf,'paperposition',[1 1 11 8])
if strcmp(str,'print')
    jprint(dirname,'overflowproblem')
end
%\*************************************************************************

%/*************************************************************************
%symmetry plots
rho=1027;
%log10delta=linspace(-1,4,101);
%log10mu=linspace(-7,4,100);
log10delta=linspace(-4,4,201);
log10mu=linspace(-8,4,200);
%h=10000;
h=[16 1e2 1e3 1e4 1e5 1e6];
%h=[15.01 15.1 16 26 1e2 1e3 1e4 1e5 1e6];
fc=1e-4;

[demat,mumat]=meshgrid(10.^log10delta,10.^log10mu);
zomat=demat.^2./mumat;

A=zeros(size(demat,1),size(demat,2),length(h));
A0=zeros(size(demat,1),size(demat,2),length(h));
for i=1:length(h)
    A(:,:,i)=frac(2,mumat).*log(frac(1+h(i)./zomat,1+15./zomat));
    A0(:,:,i)=frac(2,mumat).*log(1+h(i)./zomat);
end

%Gf=vzeros(length(log10mu),length(log10delta),length(h));
%Gf0=vzeros(length(log10mu),length(log10delta),length(h));
% for i=1:length(log10mu)
%     i
%     parfor j=1:length(log10delta)
%         for k=1:6
%           Gf(i,j,k)=rho.*fc.*windtrans(-fc*24*3600,15,fc*24*3600,10.^log10delta(j),10.^log10mu(i),h(k));
%          Gf0(i,j,k)=rho.*fc.*windtrans(-fc*24*3600,0,fc*24*3600,10.^log10delta(j),10.^log10mu(i),h(k));
%         end
%     end
% end
% maxmax(abs(A-Gf))
% maxmax(abs(A0-Gf0))
% jpcolor(log10(abs(A(:,:,1)-Gf(:,:,1))./A(:,:,1)))
% %these agree to the numerical noise level

omega=[-3:.001:1]*fc;
log10zo=[-12:.1:12]';
ii=1:10:length(log10zo);

clear dei mui dei0 mui0 dei1 mui1
zo=10.^log10zo;
for j=1:length(h)
    dei{j,1}=sqrt(frac(2*zo,1).*log(frac(1+h(j)./zo,1+15./zo)));
    mui{j,1}=dei{j}.^2./zo;
    dei1{j,1}=sqrt(frac(2*zo,10).*log(frac(1+h(j)./zo,1+15./zo)));
    mui1{j,1}=dei1{j}.^2./zo;
    dei0{j,1}=sqrt(frac(2*zo,1).*log(frac(1+h(j)./zo,1)));
    mui0{j,1}=dei0{j}.^2./zo;
end
%--------------------------------------------------------------------------
figure,
clf
contourf(log10delta,log10mu,log10(A(:,:,3)),[-10:.1:10]),nocontours,hold on
contour(log10delta,log10mu,log10(A(:,:,3)),[0 0 ],'k','linewidth',1.5)
contour(log10delta,log10mu,log10(A(:,:,3)),[-10:1:10],'k','linewidth',0.5)
contour(log10delta,log10mu,log10(zomat),[-20:20],'w','linewidth',0.5)
contour(log10delta,log10mu,log10(zomat),[0 0],'w','linewidth',1.5)
plot(log10(dei{3}(1:10:end)),log10(mui{3}(1:10:end)),'ko','markerfacecolor','k','markersize',4)
caxis([-4.7 7.3]),axis equal,ylim([-7 4]),xlim([-4 4]),
ylabel('Log10 Madsen Depth $\mu$ (m)','interpreter','latex')
xlabel('Log10 Ekman Depth $\delta$ (m)','interpreter','latex')
hc=colorbar('SouthOutside');
hc.Label.String='Log10 Inertial Amplitude A (m$^{-1}$)';
hc.Label.Interpreter='latex';
hc.Ticks=[-4:1:8];
set(gcf,'paperposition',[1 1 4*1.5 5*1.5])
%text(-0.92,3.7,'M'),text(3.8,-6.8,'E')
text(-3.925,3.7,'M'),text(3.7,-6.8,'E','color','w')
if strcmp(str,'print')
    jprint(dirname,'deltamuplane')
end
%--------------------------------------------------------------------------
%figure,cellplot(dec,muc)
%figure,cellplot(dec0,muc0)
omega=[-100:.1:-8.6 -8.5:.001:-1-0.005 -1-0.005:1e-7:-1+0.005  -1+0.005:0.001:8.5 8.6:.1:100]*fc;
%omega=[-1e4:10:-1000 -1000:1:-100 -100:.1:-8.6 -8.5:.001:-1-0.005 -1-0.005:1e-7:-1+0.005  -1+0.005:0.001 :8.5 8.6:.1:100 100:1:1000 1000:10:1e4]*fc;
G=nan*zeros(length(omega),length(ii),length(h));
[Gm,Ge,Gminf,Geinf]=vzeros(length(omega),length(h));
%G0=nan*zeros(length(omega),length(ii),length(h));
%G1=nan*zeros(length(omega),length(ii),length(h));
for j=1:size(G,3)
    j
   Gm(:,j)=rho.*abs(fc).*windtrans(omega*24*3600,15,fc*24*3600,0,mui{j}(1),h(j));
   Ge(:,j)=rho.*abs(fc).*windtrans(omega*24*3600,15,fc*24*3600,dei{j}(end),0,h(j));
   parfor k=1:length(ii)
        k
        G(:,k,j)=rho.*abs(fc).*windtrans(omega*24*3600,15,fc*24*3600,dei{j}(ii(k)),mui{j}(ii(k)),h(j));
         %G1(:,k,j)=rho.*abs(fc).*windtrans(omega*24*3600,15,fc*24*3600,dei1{j}(ii(k)),mui1{j}(ii(k)),h(j));
        %G0(:,k,j)=rho.*abs(fc).*windtrans(omega*24*3600,0,fc*24*3600,dei0{j}(ii(k)),mui0{j}(ii(k)),h(j));
    end
end
%--------------------------------------------------------------------------
figure
clf
%[~,jj]=min(abs(omega));
for j=1:size(G,3)
    subplot(2,3,j)
    hl=plot(G(:,:,j));axis equal, axis square
    axis([-.3 1 -.65 .65]*1.05),vlines(0,'0.5k:'),hlines(0,'0.5k:')
    linestyle(hl,'0.5D')
    hl=plot(G(:,1,j));linestyle(hl,'2k--')
    hl=plot(G(:,end,j));linestyle(hl,'2k')
    hl=plot(Ge(:,j));linestyle(hl,'0.5w')
    hl=plot(Gm(:,j));linestyle(hl,'0.5k')
    text(0.9,0.59,['(' setstr(96+j) ')'])
%    xlabel('Real Part of Transfer Function (m^2s / kg)')%(Dimensionless)')
%    ylabel('Imaginary Part of Transfer Function (m^2s / kg)')% (Dimensionless)')
    xlabel('Real Part of $\widetilde G(\omega,15$ m$)$ (m$^{-1}$)','interpreter','latex')%(Dimensionless)')
    ylabel('Imaginary Part of $\widetilde G(\omega,15$ m$)$ (m$^{-1}$)','interpreter','latex')% (Dimensionless)')
    xtick([-.4:.2:1.2]),ytick([-.6:.2:.6])
    %plot(G(jj,:,j),'ko','markerfacecolor','k','markersize',4)
end
packfig(2,3)
set(gcf,'paperposition',[1 1 9.4 6])
fontsize 10 10 9 9
if strcmp(str,'print')
    jprint(dirname,'similarflowers')
end
%%check invariance
%figure,plot(G(:,:,1)),hold on,plot(G1(:,:,1)/10,'r')
%figure,plot(G(:,:,end)),hold on,plot(G1(:,:,end)/10,'r')
% %--------------------------------------------------------------------------
% figure
% clf
% for j=1:size(Gf,3)
%     subplot(2,3,j)
%     hl=plot(omega./fc,abs(G(:,2:end-1,j)));hold on
%     axis([-1.99 0 1.001*1e-4 1.25]),ylog
%     linestyle(hl,'0.5D')
%     hl=plot(omega./fc,abs(G(:,1,j)));
%     linestyle(hl,'2k')
%     hl=plot(omega./fc,abs(G(:,end,j)));
%     linestyle(hl,'2k--')
%     ylabel('Log10 Magnitude of $G(\omega,15$ m$)$','interpreter','latex')
%     xlabel('Nondimensional Frequency $\omega/f$','interpreter','latex')
%     text(-.17,0.8,['(' setstr(96+j) ')'])
%     xtick([-2:.5:0])
%     vlines(-1,'0.5k:')
% end
% packfig(2,3)
% set(gcf,'paperposition',[1 1 9*1.25 5*1.25])
% jprint(dirname,'similarmagnitudes')
% %--------------------------------------------------------------------------
% figure
% clf
% for j=1:size(Gf,3)
%     subplot(2,3,j)
%     hl=plot(omega./fc,(360/2/pi)*unwrap(angle(G(:,2:end-1,j))));hold on
%     axis([-1.99 0 -179.9 180]),
%     linestyle(hl,'0.5D')
%     hl=plot(omega./fc,(360/2/pi)*unwrap(angle(G(:,1,j))));
%     linestyle(hl,'2k')
%     hl=plot(omega./fc,(360/2/pi)*unwrap(angle(G(:,end,j))));
%     linestyle(hl,'2k--')
%     ylabel('Angle of $G(\omega,15$ m$) (degrees) $','interpreter','latex')
%     xlabel('Nondimensional Frequency $\omega/f$','interpreter','latex')
%     %text(-.17,0.8,['(' setstr(96+j) ')'])
%     xtick([-2:.5:0])
%     vlines(-1,'0.5k:')
% end
% packfig(2,3)
% set(gcf,'paperposition',[1 1 9*1.25 5*1.25])
% jprint(dirname,'similarphases')
%--------------------------------------------------------------------------
% figure
% clf
% for j=1:size(Gf0,3)
%     subplot(2,3,j)
%     hl=plot(G0(:,2:end-1,j));axis equal, axis square
%     axis([-.05 1.05 -.505 .505]),vlines(0,'0.5k:'),hlines(0,'0.5k:')
%     linestyle(hl,'0.5D'),
%     hl=plot(G0(:,end,j)); linestyle(hl,'2k')
%     hl=plot(G0(:,1,j));linestyle(hl,'2k--')
%     text(0.9,0.44,['(' setstr(96+j) ')'])
%     xlabel('Real Part of $G(\omega,0$ m$)$','interpreter','latex')%(Dimensionless)')
%     ylabel('Imaginary Part of $G(\omega,0$ m$)$','interpreter','latex')% (Dimensionless)')
%     xtick([-.4:.2:1.2]),ytick([-.6:.2:.6])
% end
% packfig(2,3)
% set(gcf,'paperposition',[1 1 9.4 6])
% jprint(dirname,'similarflowers0')
lnomega=logspace(2,100,10000);
omega=[-fliplr(lnomega) -100:.1:-8.6 -8.5:.001:-1-0.005 -1-0.005:1e-7:-1+0.005  -1+0.005:0.001 :8.5 8.6:.1:100 lnomega]*fc;
G0=vzeros(length(omega),length(ii));
Gm0=rho.*abs(fc).*windtrans(omega*24*3600,0,fc*24*3600,0,mui0{3}(1),h(3));
Ge0=rho.*abs(fc).*windtrans(omega*24*3600,0,fc*24*3600,dei0{3}(end),0,h(3));
%Gm0=rho.*abs(fc).*windtrans(omega*24*3600,1e-10,fc*24*3600,0,mui0{3}(ii(end)),h(3));
%Ge0=rho.*abs(fc).*windtrans(omega*24*3600,0,fc*24*3600,dei0{3}(ii(1)),0,h(3));
%zeta=2*sqrt(2)*frac(zo,delta).*sqrt(abs(1+ommat./fc));

for k=1:length(ii)
    k
    G0(:,k)=rho.*abs(fc).*windtrans(omega*24*3600,0,fc*24*3600,dei0{3}(ii(k)),mui0{3}(ii(k)),h(3));
 %   G1(:,k)=rho.*abs(fc).*windtrans(omega*24*3600,0,fc*24*3600,dei0{3}(ii(k)),mui0{3}(ii(k)),h(1));
end
figure
clf
hl=plot(G0);axis equal, axis square
axis([-.05 1.05 -.505 .505]),vlines(0,'0.5k:'),hlines(0,'0.5k:')
linestyle(hl,'0.5D'),
hl=plot(G0(:,1)); linestyle(hl,'2k--')
hl=plot(G0(:,end)); linestyle(hl,'2k')
hl=plot(Ge0); linestyle(hl,'0.5w')
hl=plot(Gm0);linestyle(hl,'0.5k')
%text(0.9,0.44,['(' setstr(96+j) ')'])
xlabel('Real Part of $\widetilde G(\omega,0$ m$)$ (m$^{-1}$)','interpreter','latex')%(Dimensionless)')
ylabel('Imaginary Part of $\widetilde G(\omega,0$ m$)$ (m$^{-1}$)','interpreter','latex')% (Dimensionless)')
xtick([-.4:.2:1.2]),ytick([-.6:.2:.6])
set(gcf,'paperposition',[1 1 4 4])
plot(2*h(3)/squared(dei0{3}(end)),0,'wo','markerfacecolor','k')
fontsize 10 10 9 9
if strcmp(str,'print')
    jprint(dirname,'similarflowers0')
end


function T = EinsteinSum(varargin)
% tensor multiplication according to Einstein summation convention
%
% Description
% This function computes a tensor product according to Einstein summation
% convention
%
% Syntax
%   % sumation against dimension 1 and 2
%   C = EinsteinSum(E,[1 -1 2 -2],v,-1,v,-2)
%
%   eps = EinsteinSum(C,[-1 1 -2 2],sigma,[-1 -2])
%
% Input
%  T1,T2 - @tensor
%  dimT1 - vector of indices giving the summation order in tensor 1
%  dimT2 - vector of indices giving the summation order in tensor 2
%
% Output
%  T - @tensor
%
% See also
%

% TODO: check for correct symmetries !!!

global useBSXFUN;

M1 = 1; dimT1 = [];
T = varargin{1};

% for each tensor in varargin
iv = 1;
while iv < length(varargin) && ~ischar(varargin{iv})

  % take new vector from varargin
  M2 = varargin{iv};
  dimT2 = varargin{iv+1};
  iv = iv+2;

  % convert to double
  if isa(M2,'tensor')
    M2 = M2.M;
  elseif isa(M2,'quaternion')
    M2 = matrix(M2);
  elseif isa(M2,'vector3d')
    M2 = double(M2);
    M2 = permute(M2,[3 1 2]);
  end

  %
  if length(dimT2) > 1 && sum(dimT2<0)>1 && max(accumarray(-dimT2(dimT2<0).',1)>1)
    [M2,dimT2] = innerSum(M2,dimT2);
  end

  % reorder T1 such that [-rDel ... -3 -2 -1  1 2 3 .. rOut x x x]
  rOut = max([0,dimT1,dimT2]);
  rDel = -min([0,dimT1,dimT2]);
  rExt = max([0,ndims(M1) - length(dimT1), ndims(M2) - length(dimT2)]);

  % new order of dimensions for tensor 1
  dimT1(dimT1>0) = dimT1(dimT1>0) - 1;  % we should start with zero to avoid the cap
  exp1 = 1:rOut + rDel; exp1(dimT1 + rDel + 1) = []; % dimensions in reserve
  order1 = [dimT1 + rDel + 1,...  the tensor components
    rOut + rDel + (1:rExt),...    none tensor components
    exp1];

  % new order of dimensions for tensor 2
  dimT2(dimT2>0) = dimT2(dimT2>0) - 1; % we should start with zero to avoid the cap
  exp2 = 1:rOut + rDel; exp2(dimT2 + rDel + 1) = []; % dimensions in reserve
  order2 = [dimT2 + rDel + 1,...  the tensor components
    rOut + rDel + (1:rExt),...    none tensor components
    exp2];

  M1 = ipermute(M1,order1);
  M2 = ipermute(M2,order2);

  if useBSXFUN
    M1 = bsxfun(@times, M1, M2);
  else
    M1 = M1 .* M2;
  end

  % setup dimT1
  dimT1 = [-rDel:-1,1:rOut];

end

% sum over the dimensions to be removed
if useBSXFUN
  for d = 1:rDel, M1 = sum(M1,d); end
else
  if rDel>0, M1 = sum(M1,1:rDel); end
end

% and remove these leading dimensions
s = size(M1);
M1 = reshape(M1,[s(rDel+1:end) 1 1]);

% rank 0 tensor should become double
if rOut == 0
  T = M1;
elseif strcmp(class(T),'tensor') || check_option(varargin(iv:end),'keepClass')
  T.M = M1;
  T.rank = rOut;
else
  T = tensor(M1,T.CS,'noCheck','rank',rOut,varargin{iv:end});
end

end

%%
function [M,ind] = innerSum(M,ind)

% find indices to be summed
[a,b] = findDouble(ind);

if isempty(a), return;end

ind([a,b]) = [];

% remember size of matrix
sM = size(M);
sM([a,b]) = [];

% make a,b the first two dimensions
order = 1:max([ndims(M) a b]);
order([a b]) = [];
order = [a b order];
M = permute(M,order);

% reduces multiple remove dimensions to one
M = reshape(M,3^numel(a),3^numel(b),[]);

% sum along the diagonal of the first two dimensions
N = zeros(1,1,size(M,3));
for l = 1:3^numel(a)
  N = N + M(l,l,:);
end

% reshape back
M = reshape(N,[sM 1 1]);

end

%%
function [a,b] = findDouble(x)

%if length(x) <= 1 || all(x>=0) || sum(x<0)<2 || max(accumarray(-x(x<0).',1))<2
%  a = [];
%  b = [];
%  return;
%end

d = bsxfun(@minus,x,x') == 0 & bsxfun(@times,x<0,x'<0);
d = d & ~tril(ones(size(d)));
[a,b] = find(d);
a = a'; b = b';

end


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (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       : nrtDomRegularize2D.m                          |
%|    #    |   VERSION    : 0.50                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 25.11.2018                                    |
%|  / 0 \  |   LAST MODIF : 25.11.2018                                    |
%| ( === ) |   SYNOPSIS   : Singular kernel regularization (in debug mode)|
%|  `---'  |                                                              |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Parameters
N   = 1e2;
gss = 3;

% Spherical mesh
circle = mshCircle(N,1);

% Square mesh
square = mshSquare(2*N,[3 3]);

% Graphical representation
figure
plot(circle)
hold on
plot(square,'w')
axis equal
alpha(0.5)

% Domain
sigma = dom(circle,gss);

% Finite elements
u = fem(circle,'P0');
v = fem(circle,'P1');

% Gren kernel
Gxy      = @(X,Y) femGreenKernel(X,Y,'[log(r)]',[]);
dyGxy{1} = @(X,Y) femGreenKernel(X,Y,'grady[log(r)]1',[]);
dyGxy{2} = @(X,Y) femGreenKernel(X,Y,'grady[log(r)]2',[]);
dyGxy{3} = @(X,Y) femGreenKernel(X,Y,'grady[log(r)]3',[]);

% Single radiation P0
M  = integral(square.vtx,sigma,Gxy,u);
Mr = regularize(square.vtx,sigma,'[log(r)]',u);
norm(M+Mr,'inf')

% Double radiation P0
M  = integral(square.vtx,sigma,dyGxy,ntimes(u));
Mr = regularize(square.vtx,sigma,'grady[log(r)]',ntimes(u));
norm(M+Mr,'inf')

% Single radiation P1
M  = integral(square.vtx,sigma,Gxy,v);
Mr = regularize(square.vtx,sigma,'[log(r)]',v);
norm(M+Mr,'inf')

% Single layer P0
M  = integral(sigma,sigma,u,Gxy,u);
Mr = regularize(sigma,sigma,u,'[log(r)]',u);
norm(M+Mr,'inf')

% Double layer P0
M  = integral(sigma,sigma,u,dyGxy,ntimes(u));
Mr = regularize(sigma,sigma,u,'grady[log(r)]',ntimes(u));
norm(M+Mr,'inf')

% Single layer P1
M  = integral(sigma,sigma,u,Gxy,v);
Mr = regularize(sigma,sigma,u,'[log(r)]',v);
norm(M+Mr,'inf')

% Hypersingular P1
M  = integral(sigma,sigma,ntimes(u),Gxy,ntimes(v));
Mr = regularize(sigma,sigma,ntimes(u),'[log(r)]',ntimes(v));
norm(M+Mr,'inf')

M  = integral(sigma,sigma,nxgrad(u),Gxy,nxgrad(v));
Mr = regularize(sigma,sigma,nxgrad(u),'[log(r)]',nxgrad(v));
norm(M+Mr,'inf')

disp('~~> Michto gypsilab !')


function out = forces_moments_wing(x, delta, wind, ~, p_drone, p_physics)

% FORCES_MOMENTS - Computes the forces and moments acting on the UAV.
%
% Inputs:
%   x     - state variables
%   delta - actuators values (angles,...)
%   wind  - wind parameters
%   P     - general parameters
% Outputs:
%   out
%       F     - forces
%       M     - moments
%       Va    - airspeed
%       alpha - angle of attack
%       beta  - sideslip angle
%       wind  - wind vector in the inertial frame
%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Variables and Inputs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% UAV velocity wrt inertial frame in body frame
v_xyz      = x(4:6);

% Euler angles
phi         = x(7);
theta       = x(8);
psi         = x(9);

% Rotation rates
p           = x(10);
q           = x(11);
r           = x(12);

% Actuators fixed-wing
de = delta(1); % delta elevator
da = delta(2); % delta aileron
dr = delta(3); % delta rudder
dt = delta(4); % delta thrust

Rbi = Rb2i(phi,theta,psi);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Wind computation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

ws_ned  = wind(1:3);  % steady wind in NED frame
wg_xyz  = wind(4:6);  % gusts along body xyz axis

% Gust in NED frame = Rib * gust in Body frame
wg_ned = Rbi * wg_xyz;

% Wind in NED = steady in NED + gust in NED
w_ned = ws_ned + wg_ned;
wn = w_ned(1);
we = w_ned(2);
wd = w_ned(3);

%Steady wind in body (xyz) frame
Rib = Rbi';
ws_xyz = Rib * ws_ned;

% Wind in body frame
w_xyz = ws_xyz + wg_xyz;

% Compute air data wrt the inertial frame, in the body frame
va_xyz = v_xyz - w_xyz;
vax = va_xyz(1);
vay = va_xyz(2);
vaz = va_xyz(3);

% Airspeed norm (=V_inifity)
Va = norm(va_xyz);

if vax ~= 0
    alpha = atan2(vaz, vax);
else
    alpha = 0;
end

if Va ~=0
    beta = asin(vay / Va);
else
    beta = 0;
end

%   Relabel
ca = cos(alpha);
sa = sin(alpha);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%     Compute forces on fixed wing
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%   Aerodynamic coefficients in function of alpha
%       Lift and Drag Coefficients
CL = p_drone.CL0 + p_drone.CL_alpha * alpha;
CD = p_drone.CD0 + p_drone.CD_alpha * alpha;

%       Lift and Drag expressed in body frame
Cx     = - CD * ca       + CL * sa;
Cx_q   = - p_drone.CD_q * ca   + p_drone.CL_q  * sa;
Cx_de  = - p_drone.CD_de * ca  + p_drone.CL_de * sa;
Cz     = - CD * sa       - CL * ca;
Cz_q   = - p_drone.CD_q  * sa  - p_drone.CL_q  * ca;
Cz_de  = - p_drone.CD_de * sa  - p_drone.CL_de * ca;

% Weight
weight = p_drone.mass * p_physics.gravity * [-sin(theta);...
    cos(theta) * sin(phi);...
    cos(theta) * cos(phi)];

% Aerodynamic forces
p_dyn = 0.5 * p_physics.rho * Va^2;

if Va ~= 0
    kc = 0.5*p_drone.c/Va;
    kb = 0.5*p_drone.b/Va;
else
    kc = 0;
    kb = 0;
end

f_drag = p_dyn * p_drone.S_wing * ...
    [Cx + Cx_q*kc*q + Cx_de*de; ...
    p_drone.CY0 + p_drone.CY_beta*beta + p_drone.CY_p*kb*p + p_drone.CY_r*kb*r + p_drone.CY_da*da + p_drone.CY_dr*dr;...
    Cz + Cz_q*kc*q + Cz_de*de];

% Thrust force
f_thrust = 0.5*p_physics.rho*p_drone.S_prop*p_drone.C_prop*...
    [(p_drone.k_motor*dt)^2-Va^2; 0; 0];

% Total force = Weight + Aerodynamic forces + Thrust force
f_tot = weight + f_drag + f_thrust;

% Compute torques on drone
torque_aerodyn = p_dyn * p_drone.S_wing * ...
    [p_drone.b*(p_drone.Cl0 + p_drone.Cl_beta*beta + p_drone.Cl_p*kb*p + p_drone.Cl_r*kb*r + p_drone.Cl_da*da + p_drone.Cl_dr*dr);...
    p_drone.c*(p_drone.Cm0 + p_drone.Cm_alpha*alpha + p_drone.Cm_q*kc*q + p_drone.Cm_de*de);...
    p_drone.b*(p_drone.Cn0 + p_drone.Cn_beta*beta + p_drone.Cn_p*kb*p + p_drone.Cn_r*kb*r + p_drone.Cn_da*da + p_drone.Cn_dr*dr)];

% Thrust torque
%   Relabel thrust coefficients
torque_thrust = [-p_drone.k_TP*(p_drone.k_omega*dt)^2; 0; 0];

% Total torque
torque_tot = torque_aerodyn + torque_thrust;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%     Create output
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

out = [f_tot; torque_tot; Va; alpha; beta; wn; we; wd];

end


function pde = elli3DcircIntf3HM(am,ap,bm,bp,kappa,r1,r2,n1,n2)
%% USAGE: polynomial solution for Poisson equation
%  Last Modified: 02/21/2020 by Xu Zhang

%% PDE Structure
pde = struct('intf',@intf,...
    'exactu1',@exactu1,'exactu2',@exactu2,'exactu3',@exactu3,...
    'um1',@um1,'um2',@um2,'um3',@um3,'up1',@up1,'up2',@up2,'up3',@up3,...
    'Dxu',@Dxu,'Dxum',@Dxum,'Dxup',@Dxup,'Dyu',@Dyu,...
    'Dyum',@Dyum,'Dyup',@Dyup,'Dzu',@Dzu,'Dzum',@Dzum,'Dzup',@Dzup,...
    'f1',@f1,'f2',@f2,'f3',@f3,...
    'fm1',@fm1,'fm2',@fm2,'fm3',@fm3,...
    'fp1',@fp1,'fp2',@fp2,'fp3',@fp3,...
    'A',@A,'Am',@Am,'Ap',@Ap,'one',@one,...
    'B',@B,'Bm',@Bm,'Bp',@Bp);

pde.am = am;
pde.ap = ap;
pde.bm = bm;
pde.bp = bp;
pde.kappa = kappa;
%% interface function
    function u = intf(x,y,z)
        u = (x.^2 + y.^2 + z.^2).^(1/2)/r1-1;
    end

%% exact solution
    function u = exactu1(x,y,z)
        u = um1(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = up1(x(id),y(id),z(id));
    end
    function u = exactu2(x,y,z)
        u = um2(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = up2(x(id),y(id),z(id));
    end
    function u = exactu3(x,y,z)
        u = um3(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = up3(x(id),y(id),z(id));
    end
    function u = um1(x,y,z)
        u = (x + n1*uker1(x,y,z).*(y-z))/am;
    end
    function u = um2(x,y,z)
        u = (y + n1*uker1(x,y,z).*(z-x))/am;
    end
    function u = um3(x,y,z)
        u = (z + n1*uker1(x,y,z).*(x-y))/am;
    end
    function u = up1(x,y,z)
        u = (x + n2*uker1(x,y,z).*uker2(x,y,z).*(y-z))/ap;
    end
    function u = up2(x,y,z)
        u = (y + n2*uker1(x,y,z).*uker2(x,y,z).*(z-x))/ap;
    end
    function u = up3(x,y,z)
        u = (z + n2*uker1(x,y,z).*uker2(x,y,z).*(x-y))/ap;
    end
    function u = uker1(x,y,z)
        u = r1^2 - (x.^2 + y.^2 + z.^2);
    end
    function u = uker2(x,y,z)
        u = r2^2 - (x.^2 + y.^2 + z.^2);
    end
%% Boundary Function
    function u = gD1(x,y,z)
        u = exactu1(x,y,z);
    end
    function u = gD2(x,y,z)
        u = exactu2(x,y,z);
    end
    function u = gD3(x,y,z)
        u = exactu3(x,y,z);
    end
%% Derivative of the exact solution
    function u = Dxu(x,y,z)
        u = Dxum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dxup(x(id),y(id),z(id));
    end
    function u = Dyu(x,y,z)
        u = Dyum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dyup(x(id),y(id),z(id));
    end
    function u = Dzu(x,y,z)
        u = Dzum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dzup(x(id),y(id),z(id));
    end
    function u = Dxum(x,y,z)
        u = -2*n1/am*(x.*y+x.*z-y.^2-z.^2 + uker1(x,y,z));
    end
    function u = Dyum(x,y,z)
        u = -2*n1/am*(y.*z+x.*y-z.^2-x.^2 + uker1(x,y,z));
    end
    function u = Dzum(x,y,z)
        u = -2*n1/am*(x.*z+y.*z-x.^2-y.^2 + uker1(x,y,z));
    end
    function u = Dxup(x,y,z)
        u = -2*n2*uker2(x,y,z)./ap.*(x.*y+x.*z-y.^2-z.^2 + uker1(x,y,z)) - ...
            2*n2*uker1(x,y,z).*(x.*y+x.*z - y.^2-z.^2)/ap;
    end
    function u = Dyup(x,y,z)
        u = -2*n2*uker2(x,y,z)./ap.*(y.*z+x.*y-z.^2-x.^2 + uker1(x,y,z)) - ...
            2*n2*uker1(x,y,z).*(y.*z+x.*y - x.^2-z.^2)/ap;
    end
    function u = Dzup(x,y,z)
        u = -2*n2*uker2(x,y,z)./ap.*(x.*z+y.*z-x.^2-y.^2 + uker1(x,y,z)) - ...
            2*n2*uker1(x,y,z).*(x.*z+y.*z - x.^2-y.^2)/ap;
    end

%% right hand side function
    function u = f1(x,y,z)
        u = fm1(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = fp1(x(id),y(id),z(id));
    end
    function u = f2(x,y,z)
        u = fm2(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = fp2(x(id),y(id),z(id));
    end
    function u = f3(x,y,z)
        u = fm3(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = fp3(x(id),y(id),z(id));
    end

    function u = fm1(x,y,z)
        u = -10*n1*(z-y)- kappa*bm*um1(x,y,z);
    end
    function u = fm2(x,y,z)
        u = -10*n1*(x-z) - kappa*bm*um2(x,y,z);
    end
    function u = fm3(x,y,z)
        u = -10*n1*(y-x) - kappa*bm*um3(x,y,z);
    end
    function u = fp1(x,y,z)
        u = -10*n2*uker2(x,y,z).*(z-y) +8*n2*(z.^3-y.^3+z.*y.^2+z.*x.^2-y.*x.^2-y.*z.^2) -...
             10*n2*uker1(x,y,z).*(z-y) - kappa*bp*up1(x,y,z);
    end
    function u = fp2(x,y,z)
        u = -10*n2*uker2(x,y,z).*(x-z) +8*n2*(x.^3-z.^3+x.*z.^2+x.*y.^2-z.*y.^2-z.*x.^2) -...
             10*n2*uker1(x,y,z).*(x-z) - kappa*bp*up2(x,y,z);
    end
    function u = fp3(x,y,z)
        u = -10*n2*uker2(x,y,z).*(y-x) +8*n2*(y.^3-x.^3+y.*x.^2+y.*z.^2-x.*z.^2-x.*y.^2) -...
             10*n2*uker1(x,y,z).*(y-x) - kappa*bp*up3(x,y,z);
    end

%% Diffusion coefficient function
    function u = A(x,y,z)
        u = Am(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Ap(x(id),y(id),z(id));
    end
    function u = Am(x,y,z)
        u = am*ones(size(x));
    end
    function u = Ap(x,y,z)
        u = ap*ones(size(x));
    end

%% Mass coefficient function
    function u = B(x,y,z)
        u = Bm(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Bp(x(id),y(id),z(id));
    end
    function u = Bm(x,y,z)
        u = bm*ones(size(x));
    end
    function u = Bp(x,y,z)
        u = bp*ones(size(x));
    end

%% Other function
    function u = one(x,y,z)
        u = ones(size(x));
    end
end


function LR_chi2=drxlr_get_lrt(y,mu)
% compute the likelihood ratio test
% written by Issam El Naqa
%
% Copyright 2010, Joseph O. Deasy, on behalf of the DREES development team.
%
% This file is part of the Dose Response Explorer System (DREES).
%
% DREES development has been led by:  Issam El Naqa, Aditya Apte, Gita Suneja, and Joseph O. Deasy.
%
% DREES has been financially supported by the US National Institutes of Health under multiple grants.
%
% DREES is distributed under the terms of the Lesser GNU Public License.
%
%     This version of DREES 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.
%
% DREES 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 DREES.  If not, see <http://www.gnu.org/licenses/>.

seps = sqrt(eps);
n=length(y);
n1=sum(y); n0=sum(1-y);
Ls=n1*log(n1)+n0*log(n0)-n*log(n); % saturated
L=sum(y.*log(mu+seps)+(1-y).*log(1-mu+seps)); % fitted
LR_chi2=2*(L-Ls); % follows centered chi2 stats
return


function [ ap, ipvt, info ] = chpfa ( ap, n )

%*****************************************************************************80
%
%% CHPFA factors a complex hermitian packed matrix.
%
%  Discussion:
%
%    To solve A*X = B, follow CHPFA by CHPSL.
%
%    To compute inverse(A)*C, follow CHPFA by CHPSL.
%
%    To compute determinant(A), follow CHPFA by CHPDI.
%
%    To compute inertia(A), follow CHPFA by CHPDI.
%
%    To compute inverse(A), follow CHPFA by CHPDI.
%
%  Packed storage:
%
%    The following program segment will pack the upper
%    triangle of a hermitian matrix.
%
%      k = 0
%      do j = 1, n
%        do i = 1, j
%          k = k + 1
%          ap(k) = a(i,j)
%        end
%      end
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, complex AP(N*(N+1)/2); the packed form
%    of a hermitian matrix.  The columns of the upper triangle are
%    stored sequentially in a one-dimensional array.
%
%    Input, integer N, the order of the matrix.
%
%    Output, complex AP(N*(N+1)/2); a block diagonal matrix and the
%    multipliers which were used to obtain it stored in packed form.
%    The factorization can be written A = U*D*hermitian(U) where U is a
%    product of permutation and unit upper triangular matrices, hermitian(U)
%    is the conjugate transpose of U, and D is block diagonal with 1 by 1
%    and 2 by 2 blocks.
%
%    Output, integer IPVT(N), the pivot indices.
%
%    Output, integer INFO.
%    0, normal value.
%    K, if the K-th pivot block is singular.  This is not an error condition
%    for this subroutine, but it does indicate that CHPSL or CHPDI may divide
%    by zero if called.
%

%
%  Initialize.
%
%  ALPHA is used in choosing pivot block size.
%
  alpha = ( 1.0 + sqrt ( 17.0 ) ) / 8.0;

  info = 0;
%
%  Main loop on K, which goes from N to 1.
%
  k = n;
  ik = ( n * ( n - 1 ) ) / 2;

  while ( 1 )
%
%  Leave the loop if K = 0 or K = 1.
%
    if ( k == 0 )
      break
    end

    if ( k == 1 )
      ipvt(1) = 1;
      if ( cabs1 ( ap(1) ) == 0.0 )
        info = 1;
      end
      break
    end
%
%  This section of code determines the kind of
%  elimination to be performed.  When it is completed,
%  KSTEP will be set to the size of the pivot block, and
%  SWAP will be set to TRUE if an interchange is
%  required.
%
    km1 = k - 1;
    kk = ik + k;
    absakk = cabs1 ( ap(kk) );
%
%  Determine the largest off-diagonal element in column K.
%
    imax = icamax ( k-1, ap(ik+1:ik+k-1), 1 );
    imk = ik + imax;
    colmax = cabs1 ( ap(imk) );

    if ( alpha * colmax <= absakk )

      kstep = 1;
      swap = 0;
%
%  Determine the largest off-diagonal element in row IMAX.
%
    else

      rowmax = 0.0;
      im = ( imax * ( imax - 1 ) ) / 2;
      imj = im + 2 * imax;

      for j = imax + 1 : k
        rowmax = max ( rowmax, cabs1 ( ap(imj) ) );
        imj = imj + j;
      end

      if ( imax ~= 1 )
        jmax = icamax ( imax-1, ap(im+1:im+imax-1), 1 );
        jmim = jmax + im;
        rowmax = max ( rowmax, cabs1 ( ap(jmim) ) );
      end

      imim = imax + im;

      if ( alpha * rowmax <= cabs1 ( ap(imim) ) )
        kstep = 1;
        swap = 1;
      elseif ( alpha * colmax * ( colmax / rowmax ) <= absakk )
        kstep = 1;
        swap = 0;
      else
        kstep = 2;
        swap = ( imax ~= km1 );
      end

    end
%
%  Column K is zero.  Set INFO and iterate the loop.
%
    if ( max ( absakk, colmax ) == 0.0 )
      ipvt(k) = k;
      info = k;
      ik = ik - ( k - 1 );
      if ( kstep == 2 )
        ik = ik - ( k - 2 );
      end
      k = k - kstep;
      continue
    end

    if ( kstep ~= 2 )
%
%  1 x 1 pivot block.
%
      if ( swap )

        temp             = ap(im+1:im+imax);
        ap(im+1:im+imax) = ap(ik+1:ik+imax);
        ap(ik+1:ik+imax) = temp;

        imj = ik + imax;

        for jj = imax : k
          j = k + imax - jj;
          jk = ik + j;
          t       = conj ( ap(jk) );
          ap(jk)  = conj ( ap(imj) );
          ap(imj) = t;
          imj = imj - ( j - 1 );
        end

      end
%
%  Perform the elimination.
%
      ij = ik - ( k - 1 );
      for jj = 1 : km1
        j = k - jj;
        jk = ik + j;
        mulk = -ap(jk) / ap(kk);
        t = conj ( mulk );
        ap(ij+1:ij+j) = ap(ij+1:ij+j) + t * ap(ik+1:ik+j);
        ijj = ij + j;
        ap(ijj) = real ( ap(ijj) );
        ap(jk) = mulk;
        ij = ij - ( j - 1 );
      end
%
%  Set the pivot array.
%
      if ( swap )
        ipvt(k) = imax;
      else
        ipvt(k) = k;
      end
%
%  2 x 2 pivot block.
%
    else

      km1k = ik + k - 1;
      ikm1 = ik - ( k - 1 );

      if ( swap )

        temp                 = ap(im+1:im+imax);
        ap(im+1:im+imax)     = ap(ikm1+1:ikm1+imax);
        ap(ikm1+1:ikm1+imax) = temp;

        imj = ikm1 + imax;

        for jj = imax : km1
          j = km1 + imax - jj;
          jkm1 = ikm1 + j;
          t        = conj ( ap(jkm1) );
          ap(jkm1) = conj ( ap(imj) );
          ap(imj)  = t;
          imj = imj - ( j - 1 );
        end

        t        = ap(km1k);
        ap(km1k) = ap(imk);
        ap(imk)  = t;

      end
%
%  Perform the elimination.
%
      km2 = k - 2;

      if ( km2 ~= 0 )

        ak = ap(kk) / ap(km1k);
        km1km1 = ikm1 + k - 1;
        akm1 = ap(km1km1) / conj ( ap(km1k) );
        denom = 1.0 - ak * akm1;
        ij = ik - ( k - 1 ) - ( k - 2 );

        for jj = 1 : km2
          j = km1 - jj;
          jk = ik + j;
          bk = ap(jk) / ap(km1k);
          jkm1 = ikm1 + j;
          bkm1 = ap(jkm1) / conj ( ap(km1k) );
          mulk = ( akm1 * bk - bkm1 ) / denom;
          mulkm1 = ( ak * bkm1 - bk ) / denom;
          t = conj ( mulk );
          ap(ij+1:ij+j) = ap(ij+1:ij+j) + t * ap(ik+1:ik+j);
          t = conj ( mulkm1 );
          ap(ij+1:ij+j) = ap(ij+1:ij+j) + t * ap(ikm1+1:ikm1+j);
          ap(jk) = mulk;
          ap(jkm1) = mulkm1;
          ijj = ij + j;
          ap(ijj) = real ( ap(ijj) );
          ij = ij - ( j - 1 );
        end

      end
%
%  Set the pivot array.
%
      if ( swap )
        ipvt(k) = -imax;
      else
        ipvt(k) = 1 - k;
      end

      ipvt(k-1) = ipvt(k);

    end

    ik = ik - ( k - 1 );
    if ( kstep == 2 )
      ik = ik - ( k - 2 );
    end

    k = k - kstep;

  end

  return
end


function S=test(x)
% Edgar & Himmelblau, 1988
% x0 = [1, 2]'
% xo = [0, 0]'
% S(xo) = 0

S=4*x(1).^2-2.*x(1).*x(2)+x(2).^2;


function pde = elli3DcircIntf2(am,ap,bm,bp,r,x0,y0,z0,a11,a12,a)
%% USAGE: polynomial solution for Poisson equation
%  Last Modified: 02/21/2020 by Xu Zhang

%% PDE Structure
pde = struct('intf',@intf,...
    'exactu1',@exactu1,'exactu2',@exactu2,'exactu3',@exactu3,...
    'um1',@um1,'um2',@um2,'um3',@um3,'up1',@up1,'up2',@up2,'up3',@up3,...
    'Dxu',@Dxu,'Dxum',@Dxum,'Dxup',@Dxup,'Dyu',@Dyu,...
    'Dyum',@Dyum,'Dyup',@Dyup,'Dzu',@Dzu,'Dzum',@Dzum,'Dzup',@Dzup,...
    'f1',@f1,'f2',@f2,'f3',@f3,...
    'fm1',@fm1,'fm2',@fm2,'fm3',@fm3,...
    'fp1',@fp1,'fp2',@fp2,'fp3',@fp3,...
    'A',@A,'Am',@Am,'Ap',@Ap,'one',@one,...
    'B',@B,'Bm',@Bm,'Bp',@Bp);

pde.am = am;
pde.ap = ap;
pde.bm = bm;
pde.bp = bp;
%% interface function
    function u = intf(x,y,z)
        u = ((x-x0).^2 + (y-y0).^2 + (z-z0).^2).^(1/2)/r-1;
    end

%% exact solution
    function u = exactu1(x,y,z)
        u = um1(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = up1(x(id),y(id),z(id));
    end
    function u = exactu2(x,y,z)
        u = um2(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = up2(x(id),y(id),z(id));
    end
    function u = exactu3(x,y,z)
        u = um3(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = up3(x(id),y(id),z(id));
    end
    function u = um1(x,y,z)
        u1 = uker1(x,y,z).*(x-x0);
        u2 = uker2(x,y,z);
        u = (u1 + u2)/bm;
    end
    function u = um2(x,y,z)
        u1 = uker1(x,y,z).*(y-y0);
        u2 = uker2(x,y,z);
        u = (u1 + u2)/bm;
    end
    function u = um3(x,y,z)
        u1 = uker1(x,y,z).*(z-z0);
        u2 = uker2(x,y,z);
        u = (u1 + u2)/bm;
    end
    function u = up1(x,y,z)
        u1 = uker1(x,y,z).*(x-x0);
        u2 = uker2(x,y,z);
        u = (u1 + u2)/bp;
    end
    function u = up2(x,y,z)
        u1 = uker1(x,y,z).*(y-y0);
        u2 = uker2(x,y,z);
        u = (u1 + u2)/bp;
    end
    function u = up3(x,y,z)
        u1 = uker1(x,y,z).*(z-z0);
        u2 = uker2(x,y,z);
        u = (u1 + u2)/bp;
    end
    function u = uker1(x,y,z)
        u = nthroot((x-x0).^2 + (y-y0).^2 + (z-z0).^2 - r^2,a11);
        u = u.^a12;
    end
    function u = uker2(x,y,z)
        u = ((x-x0).^2 + (y-y0).^2 + (z-z0).^2 - r^2).^a;
    end
%% Boundary Function
    function u = gD1(x,y,z)
        u = exactu1(x,y,z);
    end
    function u = gD2(x,y,z)
        u = exactu2(x,y,z);
    end
    function u = gD3(x,y,z)
        u = exactu3(x,y,z);
    end
%% Derivative of the exact solution
    function u = Dxu(x,y,z)
        u = Dxum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dxup(x(id),y(id),z(id));
    end
    function u = Dyu(x,y,z)
        u = Dyum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dyup(x(id),y(id),z(id));
    end
    function u = Dzu(x,y,z)
        u = Dzum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dzup(x(id),y(id),z(id));
    end
    function u = Dxum(x,y,z)
        u = (Duker(x,y,z).*(y-y0)*2 - Duker(x,y,z).*(z-z0)*2)/bm;
    end
    function u = Dyum(x,y,z)
        u = (Duker(x,y,z).*(z-z0)*2 - Duker(x,y,z).*(x-x0)*2)/bm;
    end
    function u = Dzum(x,y,z)
        u = (Duker(x,y,z).*(x-x0)*2 - Duker(x,y,z).*(y-y0)*2)/bm;
    end
    function u = Dxup(x,y,z)
        u = (Duker(x,y,z).*(y-y0)*2 - Duker(x,y,z).*(z-z0)*2)/bp;
    end
    function u = Dyup(x,y,z)
        u = (Duker(x,y,z).*(z-z0)*2 - Duker(x,y,z).*(x-x0)*2)/bp;
    end
    function u = Dzup(x,y,z)
        u = (Duker(x,y,z).*(x-x0)*2 - Duker(x,y,z).*(y-y0)*2)/bp;
    end

    function u = Duker(x,y,z)
        u = a*((x-x0).^2 + (y-y0).^2 + (z-z0).^2 - r^2).^(a-1);
    end

%% right hand side function
    function u = f1(x,y,z)
        u = fm1(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = fp1(x(id),y(id),z(id));
    end
    function u = f2(x,y,z)
        u = fm2(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = fp2(x(id),y(id),z(id));
    end
    function u = f3(x,y,z)
        u = fm3(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = fp3(x(id),y(id),z(id));
    end

    function u = fm1(x,y,z)
        xh = x-x0; yh = y-y0; zh = z-z0;
        u = (-4*Duker(x,y,z) + DDuker(x,y,z).*(xh.*(yh+zh)-yh.^2-zh.^2)) + bm*um1(x,y,z);
    end
    function u = fm2(x,y,z)
        xh = x-x0; yh = y-y0; zh = z-z0;
        u = (-4*Duker(x,y,z) + DDuker(x,y,z).*(yh.*(xh+zh)-xh.^2-zh.^2)) + bm*um2(x,y,z);
    end
    function u = fm3(x,y,z)
        xh = x-x0; yh = y-y0; zh = z-z0;
        u = (-4*Duker(x,y,z) + DDuker(x,y,z).*(zh.*(xh+yh)-xh.^2-yh.^2)) + bm*um3(x,y,z);
    end
    function u = fp1(x,y,z)
        xh = x-x0; yh = y-y0; zh = z-z0;
        u = (-4*Duker(x,y,z) + DDuker(x,y,z).*(xh.*(yh+zh)-yh.^2-zh.^2)) + bp*up1(x,y,z);
    end
    function u = fp2(x,y,z)
        xh = x-x0; yh = y-y0; zh = z-z0;
        u = (-4*Duker(x,y,z) + DDuker(x,y,z).*(yh.*(xh+zh)-xh.^2-zh.^2)) + bp*up2(x,y,z);
    end
    function u = fp3(x,y,z)
        xh = x-x0; yh = y-y0; zh = z-z0;
        u = (-4*Duker(x,y,z) + DDuker(x,y,z).*(zh.*(xh+yh)-xh.^2-yh.^2)) + bp*up3(x,y,z);
    end

    function u = DDuker(x,y,z)
        u = a*(a-1)*((x-x0).^2 + (y-y0).^2 + (z-z0).^2 - r^2).^(a-2);
    end

%% Diffusion coefficient function
    function u = A(x,y,z)
        u = Am(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Ap(x(id),y(id),z(id));
    end
    function u = Am(x,y,z)
        u = am*ones(size(x));
    end
    function u = Ap(x,y,z)
        u = ap*ones(size(x));
    end

%% Mass coefficient function
    function u = B(x,y,z)
        u = Bm(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Bp(x(id),y(id),z(id));
    end
    function u = Bm(x,y,z)
        u = bm*ones(size(x));
    end
    function u = Bp(x,y,z)
        u = bp*ones(size(x));
    end

%% Other function
    function u = one(x,y,z)
        u = ones(size(x));
    end
end


function [K] = ku1v1(x, y, xp, yp, hyp, i)

logsigma = hyp(1);
logthetax = hyp(2);
logthetay = hyp(3);

n_x = size(x,1);
n_y = size(y,1);
n_xp = size(xp,1);
n_yp = size(yp,1);

x = repmat(x,1,n_xp);
y = repmat(y,1,n_yp);
xp = repmat(xp',n_x,1);
yp = repmat(yp',n_y,1);

switch i

case 0

K=exp(1).^(logsigma+(-1).*logthetax+(-1).*logthetay+(-1/2).*exp(1).^((-1) ...
  .*logthetax).*(x+(-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1) ...
  .*yp).^2).*(x+(-1).*xp).*(y+(-1).*yp);

case 1 % logsigma

K=exp(1).^(logsigma+(-1).*logthetax+(-1).*logthetay+(-1/2).*exp(1).^((-1) ...
  .*logthetax).*(x+(-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1) ...
  .*yp).^2).*(x+(-1).*xp).*(y+(-1).*yp);

case 2 % logthetax

K=exp(1).^(logsigma+(-1).*logthetax+(-1).*logthetay+(-1/2).*exp(1).^((-1) ...
  .*logthetax).*(x+(-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1) ...
  .*yp).^2).*((-1)+(1/2).*exp(1).^((-1).*logthetax).*(x+(-1).*xp).^2).*(x+ ...
  (-1).*xp).*(y+(-1).*yp);

case 3 % logthetay

K=exp(1).^(logsigma+(-1).*logthetax+(-1).*logthetay+(-1/2).*exp(1).^((-1) ...
  .*logthetax).*(x+(-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1) ...
  .*yp).^2).*(x+(-1).*xp).*((-1)+(1/2).*exp(1).^((-1).*logthetay).*(y+(-1) ...
  .*yp).^2).*(y+(-1).*yp);

otherwise

        K = zeros(n_x, n_xp);
end

end


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2019.                             |
%| 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                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtFfmHelmholtzBWneu.m                        |
%|    #    |   VERSION    : 0.61                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 05.09.2019                                    |
%| ( === ) |   SYNOPSIS   : Solve neumann scatering problem with          |
%|  `---'  |                Brackage-Werner formulation                   |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Parameters
N   = 1e3
tol = 1e-3
typ = 'P1'
gss = 3
X0  = [0 0 -1]

% Spherical mesh
sphere = mshSphere(N,1);
sigma  = dom(sphere,gss);
figure
plot(sphere)
axis equal

% Radiative mesh
square     = mshSquare(5*N,[5 5]);
square.vtx = [square.vtx(:,1) zeros(size(square.vtx,1),1) square.vtx(:,2)];
hold on
plot(square)

% Frequency adjusted to maximum esge size
stp = sphere.stp;
k   = 1/stp(2)
f   = (k*340)/(2*pi)

% Incident wave
PW         = @(X) exp(1i*k*X*X0');
gradxPW{1} = @(X) 1i*k*X0(1) .* PW(X);
gradxPW{2} = @(X) 1i*k*X0(2) .* PW(X);
gradxPW{3} = @(X) 1i*k*X0(3) .* PW(X);

% Incident wave representation
plot(sphere,real(PW(sphere.vtx)))
plot(square,real(PW(square.vtx)))
title('Incident wave')
xlabel('X');   ylabel('Y');   zlabel('Z');
hold off
view(0,10)
% camlight
% material dull
% lighting phong

%%% PREPARE OPERATORS
disp('~~~~~~~~~~~~~ PREPARE OPERATORS ~~~~~~~~~~~~~')

% Green kernel function --> G(x,y) = exp(ik|x-y|)/|x-y|
Gxy      = '[exp(ikr)/r]';
gradyGxy = {'grady[exp(ikr)/r]1','grady[exp(ikr)/r]2','grady[exp(ikr)/r]3'};

% Finite elements
u = fem(sphere,typ);
v = fem(sphere,typ);

% Coupling coeff
beta = 1i*k;

% Finite element mass matrix --> \int_Sx psi(x)' psi(x) dx
Id = integral(sigma,u,v);

% Finite element boundary operator -->
% k^2 * \int_Sx \int_Sy n.psi(x) G(x,y) n.psi(y) dx dy
% - \int_Sx \int_Sy nxgrad(psi(x)) G(x,y) nxgrad(psi(y)) dx dy
tic
Hr = 1/(4*pi) .* (k^2 * regularize(sigma,sigma,ntimes(u),'[1/r]',ntimes(v)) ...
    - regularize(sigma,sigma,nxgrad(u),'[1/r]',nxgrad(v)));
H  = 1/(4*pi) .* (k^2 * integral(sigma,sigma,ntimes(u),Gxy,k,ntimes(v),tol) ...
    - integral(sigma,sigma,nxgrad(u),Gxy,k,nxgrad(v),tol)) + Hr;
toc

% Finite element boundary operator --> \int_Sx \int_Sy psi(x)' dny G(x,y) psi(y) dx dy
tic
Dtr = 1/(4*pi) .* regularize(sigma,sigma,u,'grady[1/r]',ntimes(v)).';
Dt  = 1/(4*pi) .* integral(sigma,sigma,u,gradyGxy,k,ntimes(v),tol).' + Dtr;
toc

% Final operator [1i*k*beta*(-Id/2 + Dt) - H]
tic
LHS  = beta.*(-0.5*Id + Dt) - H;
toc

% Finite element incident wave trace --> \int_Sx psi(x) dnx(pw(x)) dx
RHS = - integral(sigma,ntimes(u),gradxPW);

%%% SOLVE LINEAR PROBLEM
disp('~~~~~~~~~~~~~ SOLVE LINEAR PROBLEM ~~~~~~~~~~~~~')

% Preconditionneur ILU
tic
[L,U] = ilu(beta.*(-0.5*Id + Dtr) - Hr);
toc

% Solve linear system : [H + 1i*k*beta*(Id/2 - Dt)] = -dnP0
tic
mu = mgcr(@(V) LHS*V,RHS,[],tol,100,L,U);
toc

% Jump for derivative
lambda = beta * mu;

%%% INFINITE SOLUTION
disp('~~~~~~~~~~~~~ INFINITE RADIATION ~~~~~~~~~~~~~')

% Plane waves direction
theta = 2*pi/1e3 .* (1:1e3)';
nu    = [sin(theta),zeros(size(theta)),cos(theta)];

% Green kernel function
Ginf      = '[exp(-ikxy)]';
gradxGinf = {'gradx[exp(-ikxy)]1','gradx[exp(-ikxy)]2','gradx[exp(-ikxy)]3'};

% Finite element infinite operators
Sinf = 1/(4*pi) .* integral(nu,sigma,Ginf,k,v,tol);
Dinf = 1/(4*pi) .* integral(nu,sigma,gradxGinf,k,ntimes(v),tol);

% Finite element radiation
sol = Sinf*lambda - Dinf*mu;

% Analytical solution
ref = sphereHelmholtz('inf','neu',1,k,nu);
norm(ref-sol,2)/norm(ref,2)
norm(ref-sol,'inf')/norm(ref,'inf')

% Graphical representation
figure
plot(theta,log(abs(sol)),'b',theta,log(abs(ref)),'--r')

%%% DOMAIN SOLUTION
disp('~~~~~~~~~~~~~ RADIATION ~~~~~~~~~~~~~')

% Finite element mass matrix --> \int_Sx psi(x)' psi(x) dx
Id = integral(sigma,u,v);

% Finite element boundary operator --> \int_Sx \int_Sy psi(x)' G(x,y) psi(y) dx dy
tic
Sbnd = 1/(4*pi) .* (integral(sigma,sigma,u,Gxy,k,v,tol) + ...
    regularize(sigma,sigma,u,'[1/r]',v));
toc

% Finite element boundary operator --> \int_Sx \int_Sy psi(x)' dny G(x,y) psi(y) dx dy
tic
Dbnd = 1/(4*pi) .* (integral(sigma,sigma,u,gradyGxy,k,ntimes(v),tol) + ...
    regularize(sigma,sigma,u,'grady[1/r]',ntimes(v)));
toc

% Boundary solution
Psca = Id\(Sbnd*lambda - (0.5*Id*mu + Dbnd*mu));
Pinc = PW(u.dof);
Pbnd = Pinc + Psca;

% Finite element radiative operator --> \int_Sy G(x,y) psi(y) dy
tic
Sdom = 1/(4*pi) .* (integral(square.vtx,sigma,Gxy,k,v,tol) + ...
    regularize(square.vtx,sigma,'[1/r]',v));
toc

% Finite element radiative operator --> \int_Sx \int_Sy psi(x)' grady(G(x,y)) ny.psi(y) dx dy
tic
Ddom = 1/(4*pi) .* ( integral(square.vtx,sigma,gradyGxy,k,ntimes(v),tol) + ...
    regularize(square.vtx,sigma,'grady[1/r]',ntimes(v)) );
toc

% Domain solution
Psca = Sdom*lambda - Ddom*mu;
Pinc = PW(square.vtx);
Pdom = Pinc + Psca;

% Annulation sphere interieure
r             = sqrt(sum(square.vtx.^2,2));
Pdom(r<=1.01) = Pinc(r<=1.01);

% Graphical representation
figure
plot(sphere,abs(Pbnd))
axis equal;
hold on
plot(square,abs(Pdom))
title('Total field solution')
colorbar
hold off
view(0,10)

%%% ANAYTICAL SOLUTIONS FOR COMPARISONS
% Analytical solution
Pbnd = sphereHelmholtz('dom','neu',1,k,1.001*sphere.vtx) + PW(sphere.vtx);
Pdom = sphereHelmholtz('dom','neu',1,k,square.vtx) + PW(square.vtx);

% Solution representation
figure
plot(sphere,abs(Pbnd))
axis equal;
hold on
plot(square,abs(Pdom))
title('Analytical solution')
colorbar
hold off
view(0,10)

disp('~~> Michto gypsilab !')


function As = slsymgraph(A, symmethod)
%SLSYMGRAPH Forces symmetry of the adjacency matrix of a graph
%
% $ Syntax $
%   - As = slsymgraph(A)
%   - As = slsymgraph(A, symmethod)
%
% $ Arguments $
%   - A:            The adjacency matrix of the original graph
%   - symmethod:    The method to symmetrize the graph
%   - As:           The symmetry adjacency matrix
%
% $ Description $
%   - As = slsymgraph(A) makes a symmetry version of the adjacency matrix
%     A using default method.
%
%   - As = slsymgraph(A, symmethod) symmetrizes the adjacency matrix by
%     using the specified method.
%     \*
%     \t    Table. The method to symmetrizes adjacency matrix
%     \h       name       &              description
%             'avgor'     & Force symmetry using the following rule:
%                           if both aij and aji are non-zeros, then
%                           take their average
%                           if only one of aij and aji is non-zero, then
%                           take the non-zero one
%                           if both aij and aji are zeros, then set zero
%                           (for both logical and numeric)
%             'avgand'    & Force symmetry using the following rule:
%                           if both aij and aji are non-zeros, then
%                           take their average
%                           if either one of aij and aji is zero, then
%                           set zero
%                           (for both logical and numeric)
%             'or'        & Use or-rule: d = aij | aji
%                           (for only logical)
%             'and'       & Use and-rule: d = aij & aji
%                           (for only logical)
%             'simavg'    & make simple average: always take (aij+aji)/2
%                           (for only numeric)
%     \*
%     The default method to use is 'avgor'. You can use your own function
%     handle. It should be like the following form:
%       v = f(v1, v2)
%     v1 and v2 are arrays of equal size with corresponding values. For
%     a reasonable function, it should satisfy that:
%       f(v, v) == v && f(v1, v2) = f(v2, v1)
%
% $ Remarks $
%   - A can be full matrix or sparse matrix, and As preserves the same
%     storage form.
%
%   - A should be a square matrix.
%
%   - When 'avgor' method is applied to logical, it is equivalent to 'or'
%     when 'avgand' method is applied to logical, it is equivalent to
%     'and'.
%
% $ History $
%   - Created by Dahua Lin, on Sep 8, 2006
%

%% parse and verify input

if ndims(A) ~= 2 || size(A,1) ~= size(A,2)
    error('sltoolbox:invalidarg', ...
        'The A should be a square 2D matrix');
end
n = size(A, 1);

if nargin < 2 || isempty(symmethod)
    symmethod = 'avgor';
end

if ischar(symmethod)
    switch symmethod
        case 'avgor'
            fcs = @compsym_avgor;
        case 'avgand'
            fcs = @compsym_avgand;
        case 'or'
            fcs = @compsym_or;
            if isnumeric(A)
                error('sltoolbox:rterror', ...
                    'The or method is not applicable to numerical matrix');
            end
        case 'and'
            fcs = @compsym_and;
            if isnumeric(A)
                error('sltoolbox:rterror', ...
                    'The and method is not applicable to numerical matrix');
            end
        otherwise
            error('sltoolbox:invalidarg', ...
                'Invalid method for symmetrization: %s', method);
    end
elseif isa(symmethod, 'function_handle')
    fcs = symmethod;
else
    error('sltoolbox:invalidarg', ...
        'Invalid method for symmetrization.');
end

%% main skeleton

% prepare all indices with aij or aji non-zero

[I0, J0] = find(A);

% single out diagonal ones
is_diag = (I0 == J0);
if any(is_diag)
    inds_diag = sub2ind([n, n], I0(is_diag), J0(is_diag));
else
    inds_diag = [];
end

% process the non-diagonal ones
not_diag = ~is_diag;
clear is_diag;

if any(not_diag)
    % filter indices
    I0 = I0(not_diag);
    J0 = J0(not_diag);
    clear not_diag;

    % merge to down triangular part
    I = I0;
    J = J0;
    idx_ut = find(I0 > J0);
    if ~isempty(idx_ut)
        I(idx_ut) = J0(idx_ut);
        J(idx_ut) = I0(idx_ut);
    end
    clear I0 J0 idx_ut;

    % unique and expand to up triangular part
    inds_dt = sub2ind([n, n], I, J);
    [inds_dt, si] = unique(inds_dt);
    I = I(si);
    J = J(si);
    inds_ut = sub2ind([n, n], J, I);
    clear I J si;
else
    inds_dt = [];
    inds_ut = [];
end

% get original values

if ~isempty(inds_dt)
    v_dt = A(inds_dt);
    v_ut = A(inds_ut);
else
    v_dt = [];
    v_ut = [];
end

% compute the symmetrized value

v = fcs(v_dt, v_ut);
clear v_dt v_ut;

% combine diagonal value and non-diagonal value

if ~isempty(inds_diag)
    v_diag = A(inds_diag);
else
    v_diag = [];
end

s_inds = vertcat(inds_diag, inds_dt, inds_ut);
clear inds_diag inds_dt inds_ut;
s_vals = vertcat(v_diag, v, v);
clear v_diag v;

% create matrix

As = slmakeadjmat(n, n, s_inds, s_vals, islogical(A), issparse(A));

%% symmetry value computation functions

function vd = compsym_avgor(v1, v2)

if isnumeric(v1)
    has_both = v1 & v2;
    only_v1 = v1 & ~v2;
    only_v2 = v2 & ~v1;

    vd = zeros(size(v1));
    vd(has_both) = (v1(has_both) + v2(has_both)) / 2;
    vd(only_v1) = v1(only_v1);
    vd(only_v2) = v2(only_v2);
else
    vd = v1 | v2;
end

function vd = compsym_avgand(v1, v2)

if isnumeric(v1)
    has_both = v1 & v2;

    vd = zeros(size(v1));
    vd(has_both) = (v1(has_both) + v2(has_both)) / 2;
else
    vd = v1 & v2;
end

function vd = compsym_or(v1, v2)

vd = v1 | v2;

function vd = compsym_and(v1, v2)

vd = v1 & v2;


function By = mfBy(yc,para,flag);

if ~exist('flag','var'), flag = 'By';  end;

Omega = para.Omega;
dim   = length(Omega);
m     = para.m;
h     = Omega./m;
a     = sqrt(para.mu);
b     = sqrt(para.mu+para.lambda);

% Bu for elastic-grad
% yc -> [u1,u2,u3]
%
%     dim == 3,                     dim == 2,
%     | d11         |
%     | d21         |
%     | d31         |   | u1 |          | d11     |
%     |     d12     |   |    |          | d21     |   | u1 |
%  y= |     d22     | * | u2 |      y = |     d12 | * |    |
%     |     d32     |   |    |          |     d22 |   | u2 |
%     |         d13 |   | u3 |          | d11 d22 |
%     |         d23 |
%     |         d33 |
%     | d11 d22 d33 |

flag = ['elastic-stg-',flag,'-',int2str(dim)];

switch flag
  case 'elastic-stg-By-2'
    nc = m(1)*m(2);
    nn = (m(1)+1)*(m(2)+1);
    ns1 = (m(1)+1)*m(2);
    ns2 = m(1)*(m(2)+1);

    I11 = 1:nc;                        % d11 y1
    I21 = I11(end)+(1:nn-2*(m(1)+1));  % d21 y1
    I12 = I21(end)+(1:nn-2*(m(2)+1));  % d12 y2
    I22 = I12(end)+(1:nc);             % d22 y2
    I00 = I22(end)+(1:nc);             % div Y

    By = zeros(I00(end),1);

    y1 = reshape(yc(1:ns1),m(1)+1,m(2));
    y2 = reshape(yc(ns1+(1:ns2)),m(1),m(2)+1);

    %% d11 y1
    By(I11) = reshape(y1(2:end,:)-y1(1:end-1,:),length(I11),1)/h(1);

    %% d21 y1
    By(I21) = reshape(y1(:,2:end)-y1(:,1:end-1),length(I21),1)/h(2);

    %% d12 y2
    By(I12) = reshape(y2(2:end,:)-y2(1:end-1,:),length(I12),1)/h(1);

    %% d22 y2
    By(I22) = reshape(y2(:,2:end)-y2(:,1:end-1),length(I22),1)/h(2);

    %% d22 y2
    By(I00) = b*By(I11) + b*By(I22);
    By(1:I22(end)) = a*By(1:I22(end));

  case 'elastic-stg-By-3'

    n0 = m(1)*m(2)*m(3);
    n1 = (m(1)+1)*m(2)*m(3);
    n2 = m(1)*(m(2)+1)*m(3);
    n3 = m(1)*m(2)*(m(3)+1);

    j1 = m(1)*(m(2)+1)*(m(3)+1);
    j2 = (m(1)+1)*m(2)*(m(3)+1);
    j3 = (m(1)+1)*(m(2)+1)*m(3);

    p1 = n0 + j2 + j3;
    p2 = p1 + n0 + j1 + j3;
    p3 = p2 + n0 + j1 + j2;

    ly = 4*n0+2*(j1+j2+j3);

%     mfilename, keyboard
    [y1,y2,y3] = vec2array(yc,m,'stg');

    clear yc

    By = zeros(ly,1);

    % d11 y1 : n1 -> n0
    dummy = y1(2:end,:,:) - y1(1:end-1,:,:);
    By(1:n0) = reshape(dummy,n0,1)/h(1);

    % d21 y1 : n1 -> j3
    dummy = zeros(m(1)+1,m(2)+1,m(3));
    dummy(:,2:end-1,:) = y1(:,2:end,:) - y1(:,1:end-1,:);
    By(n0+(1:j3)) = reshape(dummy,j3,1)/h(2);

    % d31 y1 : n1 -> j2
    dummy = zeros(m(1)+1,m(2),m(3)+1);
    dummy(:,:,2:end-1) = y1(:,:,2:end) - y1(:,:,1:end-1);
    By(n0+j3+(1:j2)) = reshape(dummy,j2,1)/h(3);

    % d12 y2 : n2 -> j3
    dummy = zeros(m(1)+1,m(2)+1,m(3));
    dummy(2:end-1,:,:) = y2(2:end,:,:) - y2(1:end-1,:,:);
    By(p1+(1:j3)) = reshape(dummy,j3,1)/h(1);

    % d22 y2 : n2 -> n0
    dummy = y2(:,2:end,:) - y2(:,1:end-1,:);
    By(p1+j3+(1:n0)) = reshape(dummy,n0,1)/h(2);

    % d32 y2 : n2 -> j1
    dummy = zeros(m(1),m(2)+1,m(3)+1);
    dummy(:,:,2:end-1) = y2(:,:,2:end) - y2(:,:,1:end-1);
    By(p1+j3+n0+(1:j1)) = reshape(dummy,j1,1)/h(3);

    % d13 y3 : n3 -> j2
    dummy = zeros(m(1)+1,m(2),m(3)+1);
    dummy(2:end-1,:,:) = y3(2:end,:,:) - y3(1:end-1,:,:);
    By(p2+(1:j2)) = reshape(dummy,j2,1)/h(1);

    % d23 y3 : n3 -> j1
    dummy = zeros(m(1),m(2)+1,m(3)+1);
    dummy(:,2:end-1,:) = y3(:,2:end,:) - y3(:,1:end-1,:);
    By(p2+j2+(1:j1)) = reshape(dummy,j1,1)/h(2);

    % d33 y3 : n3 -> n0
    dummy = y3(:,:,2:end) - y3(:,:,1:end-1);
    By(p2+j2+j1+(1:n0)) = reshape(dummy,n0,1)/h(3);

    % div u
    By(end-n0+1:end) = By(1:n0)+By(p1+j3+(1:n0)) + By(p2+j2+j1+(1:n0));

    By(1:end-n0)     = a*By(1:end-n0);
    By(end-n0+1:end) = b*By(end-n0+1:end);

  case 'elastic-stg-BTy-2'

    % the input yc is the result of B*y, therefore it can be splitted into 5
    % parts: d11u1, d21u1,d12u2,d22u2,Divy

    nc  = m(1)*m(2);
    nn  = (m(1)+1)*(m(2)+1);
    ns1 = (m(1)+1)*m(2);
    ns2 = m(1)*(m(2)+1);

    By  = zeros(ns1+ns2,1);
    I11 = 1:nc;                        % d11 y1
    I21 = I11(end)+(1:nn-2*(m(1)+1));  % d21 y1
    I12 = I21(end)+(1:nn-2*(m(2)+1));  % d12 y2
    I22 = I12(end)+(1:nc);             % d22 y2
    I00 = I22(end)+(1:nc);             % div Y

    yd = reshape(yc(I11),m);
    zd = reshape([-yd(1,:);yd(1:end-1,:)-yd(2:end,:);yd(end,:)],ns1,1)/h(1);
    By(1:ns1) = a*zd;

    yd = reshape(yc(I21),m(1)+1,m(2)-1);
    zd = reshape([-yd(:,1),yd(:,1:end-1)-yd(:,2:end),yd(:,end)],ns1,1)/h(2);
    By(1:ns1) = By(1:ns1) + a*zd;

    yd = reshape(yc(I00),m);
    zd = reshape([-yd(1,:);yd(1:end-1,:)-yd(2:end,:);yd(end,:)],ns1,1)/h(1);
    By(1:ns1) = By(1:ns1) + b*zd;

    yd = reshape(yc(I12),m(1)-1,m(2)+1);
    zd = reshape([-yd(1,:);yd(1:end-1,:)-yd(2:end,:);yd(end,:)],ns2,1)/h(1);
    By(1+ns1:end) = a*zd;

    yd = reshape(yc(I22),m);
    zd = reshape([-yd(:,1),yd(:,1:end-1)-yd(:,2:end),yd(:,end)],ns2,1)/h(2);
    By(1+ns1:end) = By(1+ns1:end) + a*zd;

    yd = reshape(yc(I00),m);
    zd = reshape([-yd(:,1),yd(:,1:end-1)-yd(:,2:end),yd(:,end)],ns2,1)/h(2);
    By(1+ns1:end) = By(1+ns1:end) + b*zd;

  case 'elastic-stg-BTy-3'

    % the input yc is the result of B*y, therefore it can be splitted into 10
    % parts: d11y1, d21y1,d31y1,d12y2,d22y2,d32y2,d13y3,d23y3,u33y3,Divy

    n0 = m(1)*m(2)*m(3);
    n1 = (m(1)+1)*m(2)*m(3);
    n2 = m(1)*(m(2)+1)*m(3);
    n3 = m(1)*m(2)*(m(3)+1);

    j1 = m(1)*(m(2)+1)*(m(3)+1);
    j2 = (m(1)+1)*m(2)*(m(3)+1);
    j3 = (m(1)+1)*(m(2)+1)*m(3);

    p1 = n0 + j2 + j3;
    p2 = p1 + n0 + j1 + j3;
    p3 = p2 + n0 + j1 + j2;

    By = zeros(n1+n2+n3,1);

    % extract Divy
    Divy  = reshape(yc(end-n0+1:end),m(1),m(2),m(3));

    % ---------------------------------- part 1 of u
    % extract D11y1, boundary condition for tangential derivative
    uu = reshape(yc(1:n0),m(1),m(2),m(3));
    dummy = zeros(m(1)+1,m(2),m(3));
    dummy(1,:,:)   = -uu(1,:,:);
    dummy(end,:,:) =  uu(end,:,:);
    dummy(2:end-1,:,:) = uu(1:end-1,:,:) - uu(2:end,:,:);
    By(1:n1) = a*reshape(dummy,n1,1)/h(1);

    % extract D21y1, boundary condition for normal derivative
    uu = reshape(yc(n0+(1:j3)),m(1)+1,m(2)+1,m(3));
    uu(:,[1,end],:) = 0;
    dummy = uu(:,1:end-1,:) - uu(:,2:end,:);
    By(1:n1) = By(1:n1) + a*reshape(dummy,n1,1)/h(2);

    % extract D31u1, boundary condition for normal derivative
    uu = reshape(yc(n0+j3+(1:j2)),m(1)+1,m(2),m(3)+1);
    uu(:,:,[1,end]) = 0;
    dummy = uu(:,:,1:end-1) - uu(:,:,2:end);
    By(1:n1) = By(1:n1) + a*reshape(dummy,n1,1)/h(3);

    % Divy, boundary condition for tangential derivative
    dummy = zeros(m(1)+1,m(2),m(3));
    dummy(1,:,:)   = -Divy(1,:,:);
    dummy(end,:,:) =  Divy(end,:,:);
    dummy(2:end-1,:,:) = Divy(1:end-1,:,:) - Divy(2:end,:,:);
    By(1:n1) = By(1:n1) + b*reshape(dummy,n1,1)/h(1);

    % ---------------------------------- part 2 of y
    % extract D12y2, boundary condition for normal derivative
    uu = reshape(yc(p1+(1:j3)),m(1)+1,m(2)+1,m(3));
    uu([1,end],:,:) = 0;
    dummy = uu(1:end-1,:,:) - uu(2:end,:,:);
    By(n1+(1:n2)) = a*reshape(dummy,n2,1)/h(1);

    % extract D22y2, boundary condition for tangential derivative
    uu = reshape(yc(p1+j3+(1:n0)),m(1),m(2),m(3));
    dummy = zeros(m(1),m(2)+1,m(3));
    dummy(:,1,:)   = -uu(:,1,:);
    dummy(:,end,:) =  uu(:,end,:);
    dummy(:,2:end-1,:) = uu(:,1:end-1,:) - uu(:,2:end,:);
    By(n1+(1:n2)) = By(n1+(1:n2)) + a*reshape(dummy,n2,1)/h(2);

    % extract D32y2, boundary condition for normal derivative
    uu = reshape(yc(p1+j3+n0+(1:j1)),m(1),m(2)+1,m(3)+1);
    uu(:,:,[1,end]) = 0;
    dummy = uu(:,:,1:end-1) - uu(:,:,2:end);
    By(n1+(1:n2)) = By(n1+(1:n2)) + a*reshape(dummy,n2,1)/h(3);

    % Divy, boundary condition for tangential derivative
    dummy = zeros(m(1),m(2)+1,m(3));
    dummy(:,1,:)   = -Divy(:,1,:);
    dummy(:,end,:) =  Divy(:,end,:);
    dummy(:,2:end-1,:) = Divy(:,1:end-1,:) - Divy(:,2:end,:);
    By(n1+(1:n2)) = By(n1+(1:n2)) + b*reshape(dummy,n2,1)/h(2);

    % ---------------------------------- part 3 of y
    % extract D13y3, boundary condition for normal derivative
    uu = reshape(yc(p2+(1:j2)),m(1)+1,m(2),m(3)+1);
    uu([1,end],:,:) = 0;
    dummy = uu(1:end-1,:,:) - uu(2:end,:,:);
    By(n1+n2+(1:n3)) = a*reshape(dummy,n3,1)/h(1);

    % extract D23y3, boundary condition for normal derivative
    uu = reshape(yc(p2+j2+(1:j1)),m(1),m(2)+1,m(3)+1);
    uu(:,[1,end],:) = 0;
    dummy = uu(:,1:end-1,:) - uu(:,2:end,:);
    By(n1+n2+(1:n3)) = By(n1+n2+(1:n3)) + a*reshape(dummy,n3,1)/h(2);

    % extract D32y2, boundary condition for tangential derivative
    uu = reshape(yc(p2+j2+j1+(1:n0)),m(1),m(2),m(3));
    dummy = zeros(m(1),m(2),m(3)+1);
    dummy(:,:,1)   = -uu(:,:,1);
    dummy(:,:,end) =  uu(:,:,end);
    dummy(:,:,2:end-1) = uu(:,:,1:end-1) - uu(:,:,2:end);
    By(n1+n2+(1:n3)) = By(n1+n2+(1:n3)) + a*reshape(dummy,n3,1)/h(3);

    % Divy, boundary condition for tangential derivative
    dummy = zeros(m(1),m(2),m(3)+1);
    dummy(:,:,1)   = -Divy(:,:,1);
    dummy(:,:,end) =  Divy(:,:,end);
    dummy(:,:,2:end-1) = Divy(:,:,1:end-1) - Divy(:,:,2:end);
    By(n1+n2+(1:n3)) = By(n1+n2+(1:n3)) + b*reshape(dummy,n3,1)/h(3);

  otherwise, jmerror(flag)
end;

% ==============================================================================


function feasible = checkfeasiblefast(p,x,tol)

feasible = 0;
if isempty(x)
    return
end
if ~(all(x < p.ub+tol) && all(x > p.lb-tol))
    return
end

if ~isempty(p.F_struc)
    vecres = p.F_struc*[1;x];

    if any(p.K.f)
        if any(-abs(vecres(1:p.K.f)) < - tol)
            return
        end
    end

    if any(p.K.l)
        if any(vecres(p.K.f+1:p.K.f+p.K.l) <-tol)
            return
        end
    end

    if any(p.K.q)
        top = startofSOCPCone(p.K);
        for i = 1:length(p.K.q)
            n = p.K.q(i);
            X = vecres(top:top+n-1);top = top+n;
            if X(1)-norm(full(X(2:end))) < -tol
                return
            end
        end
    end

    if any(p.K.e)
        top = startofEXPCone(p.K);
        for i = 1:p.K.e           ;
            X = vecres(top:top+2);top = top+3;
            if X(2)==0 && X(3) < -tol
                return
            elseif X(3) - X(2)*exp(X(1)/X(2)) < -tol
                return
            end
        end
    end

    if any(p.K.p)
        top = startofPOWCone(p.K);
        for i = 1:length(p.K.p)
            X = vecres(top:top+p.K.p(i)-1);
            a = X(end);
            top = top+p.K.p(i);
            if X(1)^a*X(2)^(1-a) - norm(X(3:end-1)) < -tol
                return
            end
        end
    end

    if any(p.K.s)
        top = startofSDPCone(p.K);
        for i = 1:length(p.K.s)
            n = p.K.s(i);
            X = reshape(vecres(top:top+n^2-1),n,n);top = top+n^2;
            X = (X+X')/2 + tol*eye(n);
            [~,fail] = chol(X);
            if fail
                return
            end
        end
    end
end
feasible = 1;


function [surf, labels] = imSurfaceArea(img, varargin)
% Surface area of the regions within a 3D binary or label image.
%
%   S = imSurfaceArea(IMG)
%   Estimates the surface area of the 3D binary structure represented by
%   IMG.
%
%   S = imSurfaceArea(IMG, NDIRS)
%   Specifies the number of directions used for estimating surface area.
%   NDIRS can be either 3 or 13, default is 13.
%
%   S = imSurfaceArea(..., SPACING)
%   Specifies the spatial calibration of the image. SPACING is a 1-by-3 row
%   vector containing the voxel size in the X, Y and Z directions, in that
%   orders.
%
%   S = imSurfaceArea(LBL)
%   [S, L] = imSurface(LBL)
%   When LBL is a label image, returns the surface area of each label in
%   the 3D array, and eventually returns the indices of processed labels.
%
%   S = imSurfaceArea(..., LABELS)
%   In the case of a label image, specifies the labels of the region to
%   analyse.
%
%
%   Example
%     % Create a binary image of a ball
%     [x y z] = meshgrid(1:100, 1:100, 1:100);
%     img = sqrt( (x-50.12).^2 + (y-50.23).^2 + (z-50.34).^2) < 40;
%     % compute surface area of the ball
%     S = imSurfaceArea(img)
%     S =
%         2.0103e+04
%     % compare with theoretical value
%     Sth = 4*pi*40^2;
%     100 * (S - Sth) / Sth
%     ans =
%         -0.0167
%
%     % compute surface area of several regions in a label image
%     img = uint8(zeros(10, 10, 10));
%     img(2:3, 2:3, 2:3) = 1;
%     img(5:8, 2:3, 2:3) = 2;
%     img(5:8, 5:8, 2:3) = 4;
%     img(2:3, 5:8, 2:3) = 3;
%     img(5:8, 5:8, 5:8) = 8;
%     [surfs, labels] = imSurfaceArea(img)
%     surfs =
%        16.4774
%        29.1661
%        29.1661
%        49.2678
%        76.7824
%     labels =
%          1
%          2
%          3
%          4
%          8
%
%
%   See also
%     imVolume, imMeanBreadth, imSurfaceAreaDensity, imJointSurfaceArea
%

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2010-07-26,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRAE - Cepia Software Platform.

%% Parse input arguments

% check image dimension
if ndims(img) ~= 3
    error('first argument should be a 3D image');
end

% the labels to compute
labels = [];

% default number of directions
nDirs = 13;

% default image calibration
delta = [1 1 1];

% methods to compute direction weights. Can be {'Voronoi'}, 'isotropic'.
directionWeights = 'voronoi';

% Process user input arguments
while ~isempty(varargin)
    var1 = varargin{1};

    if isnumeric(var1)
        % option is either connectivity or resolution
        if isscalar(var1)
            nDirs = var1;
        elseif all(size(var1) == [1 3])
            delta = var1;
        elseif size(var1, 2) == 1
            labels = var1;
        end
        varargin(1) = [];

    elseif ischar(var1)
        if length(varargin) < 2
            error('optional named argument require a second argument as value');
        end
        if strcmpi(var1, 'directionweights')
            directionWeights = varargin{2};
        end
        varargin(1:2) = [];

    else
        error('option should be numeric');
    end

end

%% Process label image

% in case of a label image, return a vector with a set of results
if ~islogical(img)
    % extract labels if necessary (considers 0 as background)
    if isempty(labels)
        labels = imFindLabels(img);
    end

    % allocate result array
    nLabels = length(labels);
    surf = zeros(nLabels, 1);

    % compute bounding box of each region
    boxes = imBoundingBox(img, labels);

    % Compute surface area of each region considered as binary image
    % The computation is performed on a subset of the image for reducing
    % memory footprint.
    for i = 1:nLabels
        label = labels(i);

        % convert bounding box to image extent, in x, y and z directions
        box = boxes(i,:);
        i0 = ceil(box([3 1 5]));
        i1 = floor(box([4 2 6]));

        % crop image of current label
        bin = img(i0(1):i1(1), i0(2):i1(2), i0(3):i1(3)) == label;
        surf(i) = imSurfaceArea(bin, nDirs, delta, 'directionWeights', directionWeights);
    end

    return;
end

%% Process binary images

% in case of binary image, compute only one label...
labels = 1;

% distances between a pixel and its neighbours.
d1  = delta(1); % x
d2  = delta(2); % y
d3  = delta(3); % z

% volume of a voxel (used for computing line densities)
vol = d1 * d2 * d3;

%% Main processing for 3 directions

% number of voxels
nv = sum(img(:));

% number of connected components along the 3 main directions
% (Use Graph-based formula: chi = nVertices - nEdges)
n1 = nv - sum(sum(sum(img(:,1:end-1,:) & img(:,2:end,:)))); % x
n2 = nv - sum(sum(sum(img(1:end-1,:,:) & img(2:end,:,:)))); % y
n3 = nv - sum(sum(sum(img(:,:,1:end-1) & img(:,:,2:end)))); % z

if nDirs == 3
    % compute surface area by averaging over the 3 main directions
    surf = 4/3 * (n1/d1 + n2/d2 + n3/d3) * vol;
    return;
end

%% Additional processing for 13 directions

% Number of connected components along diagonals contained in the three
% main planes
% XY planes
n4 = nv - sum(sum(sum(img(2:end,1:end-1,:)   & img(1:end-1,2:end,:))));
n5 = nv - sum(sum(sum(img(1:end-1,1:end-1,:) & img(2:end,2:end,:))));
% XZ planes
n6 = nv - sum(sum(sum(img(:,2:end,1:end-1)   & img(:,1:end-1,2:end))));
n7 = nv - sum(sum(sum(img(:,1:end-1,1:end-1) & img(:,2:end,2:end))));
% YZ planes
n8 = nv - sum(sum(sum(img(2:end,:,1:end-1)   & img(1:end-1,:,2:end))));
n9 = nv - sum(sum(sum(img(1:end-1,:,1:end-1) & img(2:end,:,2:end))));

%TODO: add the case of 9 directions ?

% Number of connected components along lines corresponding to diagonals of
% the unit cube
n10 = nv - sum(sum(sum(img(1:end-1,1:end-1,1:end-1) & img(2:end,2:end,2:end))));
n11 = nv - sum(sum(sum(img(2:end,1:end-1,1:end-1) & img(1:end-1,2:end,2:end))));
n12 = nv - sum(sum(sum(img(1:end-1,2:end,1:end-1) & img(2:end,1:end-1,2:end))));
n13 = nv - sum(sum(sum(img(2:end,2:end,1:end-1) & img(1:end-1,1:end-1,2:end))));

% space between 2 voxels in each direction
d12  = hypot(d1, d2);
d13  = hypot(d1, d3);
d23  = hypot(d2, d3);
d123 = sqrt(d1^2 + d2^2 + d3^2);

% Compute weights corresponding to surface fraction of spherical caps
% For isotropic case, weights correspond to:
% c1 = 0.04577789120476 * 2;  % Ox
% c2 = 0.04577789120476 * 2;  % Oy
% c3 = 0.04577789120476 * 2;  % Oz
% c4 = 0.03698062787608 * 2;  % Oxy
% c6 = 0.03698062787608 * 2;  % Oxz
% c8 = 0.03698062787608 * 2;  % Oyz
% c10 = 0.03519563978232 * 2;  % Oxyz
if strcmp(directionWeights, 'isotropic')
    c = zeros(13,1);
    c(1) = 0.04577789120476 * 2;  % Ox
    c(2) = 0.04577789120476 * 2;  % Oy
    c(3) = 0.04577789120476 * 2;  % Oz
    c(4:5)  = 0.03698062787608 * 2;  % Oxy
    c(6:7)  = 0.03698062787608 * 2;  % Oxz
    c(8:9)  = 0.03698062787608 * 2;  % Oyz
    c(10:13) = 0.03519563978232 * 2;  % Oxyz

else
    c = computeDirectionWeights3d13(delta);
end

% compute the weighted sum of each direction
% intersection count * direction weight / line density
surf = 4 * vol * (...
    n1*c(1)/d1 + n2*c(2)/d2 + n3*c(3)/d3 + ...
    (n4+n5)*c(4)/d12 + (n6+n7)*c(6)/d13 + (n8+n9)*c(8)/d23 + ...
    (n10 + n11 + n12 + n13)*c(10)/d123 );


function [xo,Ot,nS]=buscarnd(S,x0,ip,nOt,samples,Lb,Ub,problem,tol,mxit,R,red,mem)
%   Unconstrained global optimization using adaptive random search.
%
%   [xo,Ot,nS]=buscarnd(S,x0,ip,nOt,samples,Lb,Ub,problem,tol,mxit,R,red,mem)
%
%   S: objective function
%   x0: initial point
%   ip: (0): no plot (default), (>0) plot figure ip with pause, (<0) plot figure ip
%   nOt: maximum number of optimal points (default = 1)
%   samples: number of samples per stage (default = 3*size(x0(:),1))
%   Lb, Ub: lower and upper bound vectors to plot (default = x0*(1+/-2))
%   problem: (-1): minimum (default), (1): maximum
%   tol: tolerance (default = 1e-4)
%   mxit: maximum number of stages (default = 50*(1+4*~(ip>0)))
%   R: axis vector of the hyperellipse centered in x0 (default = max(0.1*abs(x0+~x0),1))
%   red: factor to reduce the size of the axis vector (0,1) (default = 0.2)
%   mem: number of stored points during the exploration phase [0, min(18,samples-1)]
%   xo: matrix of optimal points (column vectors)
%   Ot: vector of optimal values of S
%   nS: number of objective function evaluations

%   Copyright (c) 2001 by LASIM-DEQUI-UFRGS
%   $Revision: 1.0 $  $Date: 2001/07/10 19:40:05 $
%   Argimiro R. Secchi (arge@enq.ufrgs.br)
%
%   Based on the algorithm of the same author written in C
%   for constrained global optimization in 1989/09/29 and published as:
%   A.R. Secchi and C.A. Perlingeiro, "Busca Aleatoria Adaptativa",
%   in Proc. of XII Congresso Nacional de Matematica Aplicada e
%   Computacional, Sao Jose do Rio Preto, SP, pp. 49-52 (1989).
%
% Modified by Giovani Tonel(giovani.tonel@ufrgs.br) on September 2006

rand('state',sum(100*clock)); 	%rand('state',sum(100*clock)) resets it to
													%		         a different state each time.

if nargin < 2,
   error('buscarnd requires two input arguments');
 end
 if nargin < 3 | isempty(ip),
   ip=0;
 end
 if nargin < 4 | isempty(nOt),
   nOt=1;
 end
 if nargin < 5 | isempty(samples),
   samples=3*size(x0(:),1);
 end
 if nargin < 6 | isempty(Lb),
   Lb=-x0-~x0;
 end
 if nargin < 7 | isempty(Ub),
   Ub=2*x0+~x0;
 end
 if nargin < 8 | isempty(problem),
   problem=-1;
 end
 if nargin < 9 | isempty(tol),
   tol=1e-4;
 end
 if nargin < 10 | isempty(mxit),
   mxit=50*(1+4*~(ip>0));
 end
 if nargin < 11 | isempty(R),
   R=max(0.1*abs(x0+~x0),1);
 end
 if nargin < 12 | isempty(red) | red <= 0 | red >= 1,
   red=0.2;
 end
 if nargin < 13 | isempty(mem),
   mem=min(18,samples-1);
 end
 if mem >= samples,
   mem=samples-1;
 end
                    % initialization
 x0=x0(:);
 R=abs(R(:));
 n=size(x0,1);
 distrib=1;          % distr =  1  --> exploration phase
                     % distr >  1  --> progression phase
 asym1=-1*ones(n,1); % asym1 =  1  --> asymmetric distribution to reduce xo
                     % asym1 = -1  --> asymmetric distribution to increase xo
 asym2=2*ones(n,1);  % asym2 =  2  --> symmetric distribution
                     % asym2 = 1.5 --> asymmetric distribution
 metric=0.5*norm(R);
 a0=0.5*metric;
 a2=0.1*metric;
 a3=10*tol;
 n3=1;
 holdm=floor(1/red+0.5)*n;
 nhold=0;            % counter to keep distribution constant during holdm times

 top=0;
 if mem > 0,
   n4=10*mem;
   idx=[1:n4+1];
   last=n4;
   first=0;
   idx(last+1)=first;
   mem_x=zeros(n,n4);
   mem_S=-ones(1,n4)*inf;
 end

%     Criterion of axis vector reduction
%
%     R_red = R*0.5^(distrib-1)
%
%     or norm(R_red) = norm(R)*0.5^(distrib-1)
%
%     then if the criterion is norm(R_red) < tol = 10^(-asc)
%
%     the distrib is limited by
%
%     distrib = 1 + (asc + log10(norm(R))) / log10(2)

 lim_distrib=floor(0.5*(1+log10(metric/tol)/log10(2.0)));

 if ip & n == 2,
   figure(abs(ip));
   [X1,X2]=meshgrid(Lb(1):(Ub(1)-Lb(1))/20:Ub(1),Lb(2):(Ub(2)-Lb(2))/20:Ub(2));
   [n1,n2]=size(X1);
   f=zeros(n1,n2);
   for i=1:n1,
    for j=1:n2,
      f(i,j)=feval(S,[X1(i,j);X2(i,j)]);
    end
   end
   mxf=max(max(f));
   mnf=min(min(f));
   df=mnf+(mxf-mnf)*(2.^(([0:10]/10).^2)-1);
   [v,h]=contour(X1,X2,f,df); hold on;
%   clabel(v,h);
   h1=plot(x0(1),x0(2),'ro');
   legend(h1,'start point');

   if ip > 0,
     disp('Pause: hit any key to continue...'); pause;
   end
 end

 xOt=[];
 Ot=[];
 xo=x0;
 yo=feval(S,x0)*problem;
 nS=1;
 it=0;
 opt=0;
 x=zeros(n,samples);
 y=zeros(1,samples);

 while it < mxit & opt < nOt,
  l3=0;
  it=it+1;
  for j=1:samples,   % sampling
    a=min(max(rand(n,1),0.05),0.95);
    x(:,j)=xo+(R.*asym1.*(asym2.*a-1).^distrib)/distrib;
    y(j)=feval(S,x(:,j))*problem;
    nS=nS+1;
  end

  l4=mem & distrib == 1;  % sorting the samples
  if samples > 1,
    if ~l4,
      [y(1),i]=max(y);
      x(:,1)=x(:,i);
    else
      [y,i]=sort(-y); y=-y;
      x=x(:,i);
    end
  end

  if l4,                  % memorize samples
    n2=top;
    l1=0;
    for j=1:samples,
      n1=0;
      if opt,
        for i=1:opt
          if norm(xOt(:,i)-x(:,j)) < a2,
            n1=1;
            if j == 1,
              l1=1;
            end
            break;
          end
        end
      end

      if ~n1,
	for i=1:j-1
	   if norm(x(:,j)-x(:,i)) < metric,
	     n1=1;
	     break;
	   end
	end

	if ~n1,
	  k=idx(1);
	  for i=1:top
             if norm(mem_x(:,k)-x(:,j)) < metric,
               n1=1;
               break;
             else
               k=idx(k+1);
             end
          end

	  if ~n1,
            first=idx(first+1);
            if ~first,              % expand memory
              mem_x=[mem_x,zeros(n,n4)];
              mem_S=[mem_S,-ones(1,n4)*inf];
              idx=[idx,[n2+1:n2+n4+1]];
              first=n2;
              idx(last+1)=first;
              last=n2+n4;
              idx(last+1)=0;
            end
            k=first;
            n2=n2+1;
          end

          if ~n1 | y(j) > mem_S(k),
            mem_x(:,k)=x(:,j);
            mem_S(:,k)=y(j);
          end

	  if l1,
            l1=0;
            x(:,1)=x(:,j);
            y(1)=y(j);
          end
        end
      end

      if n2-top == mem,
        break;
      end
    end
    top=n2;
  end
                  % analysis of best sampled point
  a1=norm(x(:,1)-xo)/(0.1+norm(xo))+abs(y(1)-yo)/(0.1+abs(yo));
  l1=y(1) > yo;
  if l1,
    if norm(x(:,1)-xo) > a0,
      nhold=0;
      n3=1;
    end

    z=xo; xo=x(:,1); x(:,1)=z;
    a4=yo; yo=y(1); y(1)=a4;

    if ip & n == 2,
      plot([x(1) xo(1)],[x(2) xo(2)],'r');
      if ip > 0,
        disp('Pause: hit any key to continue...'); pause;
      end
    end
  end

  if a1 < tol & distrib >= lim_distrib,
    l2=1;
    if opt,
      for i=1:opt,
        if norm(xOt(:,i)-xo) < a3,
          l2=0;
          break;
        end
      end
    end

    if l2,
      opt=opt+1;
      Ot=[Ot,yo];
      xOt=[xOt,xo];
      if ip & n == 2,
        h2=plot(xo(1,:),xo(2,:),'r*');
        if opt == 1,
          legend([h1,h2],'start point','optimum');
        end
      end
    end

        % rescue another point from memory
    if ~top,
      if opt,
        xo=xOt;
        yo=Ot;
      end
      break;
    end

    if l2 & ~l1,
      n2=0;
      for j=1:top,
        k=idx(n2+1);
        if norm(mem_x(:,k)-xo) < a2,
          top=top-1;
          idx(last+1)=k;
          last=k;
          idx(n2+1)=idx(k+1);
          idx(k+1)=0;
          if k == first,
            first=n2;
          end
        else
          n2=k;
        end
      end
    end

    if ~top,
      if opt,
        xo=xOt;
        yo=Ot;
      end
      break;
    end

    k=idx(1);
    xo=mem_x(:,k);
    yo=mem_S(k);
    idx(last+1)=k;
    last=k;
    idx(1)=idx(k+1);
    idx(k+1)=1;
    top=top-1;

    if k == first,
      first=1;
    end

    l3=1;

    if ip & n == 2 & opt < nOt,
      plot(xo(1),xo(2),'ro');
    end
  elseif opt & (~l1 | (l1 & ~l4))
        n1=0;
	for i=1:opt,
          if norm(xOt(:,i)-xo) < a2,
            n1=1;
            break;
          end
        end

	if n1,
          if ~top,
            xo=xOt;
            yo=Ot;
            break;
          end

          k=idx(1);
          xo=mem_x(:,k);
          yo=mem_S(k);
          idx(last+1)=k;
          last=k;
          idx(1)=idx(k+1);
          idx(k+1)=1;
          top=top-1;

          if k == first,
            first=1;
          end

	  l3=1;

          if ip & n == 2 & opt < nOt,
            plot(xo(1),xo(2),'ro');
          end
        end
  end
                 % adjust search direction and criterion
  if l3,
    n3=1;
    distrib=1;
    nhold=0;
  elseif (a1 < a2 | ~l1) & (nhold+n3 > holdm),
        n3 = n3+~mod(distrib,3);
        distrib=distrib+2;
	nhold=0;
  else
    nhold=nhold+n3;
  end

  for i=1:n,
    if l3 | (~l3 & x(i,1) == xo(i)),
      asym2(i)=2;
    else
      asym2(i)=1.5;
      if x(i,1) < xo(i),
        asym1(i)=1;
      else
        asym1(i)=-1;
      end
    end
  end
 end

 if it == mxit,
   %disp('Warning Buscarnd: reached maximum number of stages!');
   if opt,
     yo=Ot;
     xo=xOt;
   end
 end

 if opt == nOt,
   yo=Ot;
   xo=xOt;
 end

 if opt > 1,
   [yo,i]=sort(-yo); yo=-yo;
   xo=xo(:,i);
 end

 Ot=yo*problem;