function [res,err] = doubly_adaptive_optimized(f,x,varargin)

%%%%%
%
% INPUT: 
%   f:      Function to integrate
%   x:      Region 
%   Named Arguments: 
%   tol:    Tolerance
%   f:      Hint parameter
%   maxdiv: Max number of domain subdivisions
%   maxord: Maximum order for the formulas
%   err_method: Error estimation method
%           1: Higher order Gauss
%           2: Antigauss
%           3: Gauss-Kronrod
%   domain: Domain of integration
%           1: Torus
%           2: Sphere
%   coord: Coordinate system
%           0: Cartesian
%           1: Polar (spherical or toroidal)
%
% OUTPUT:
%   res:    Computed integral
%   err:    Error estimate
%
%%%%%

%# define defaults at the beginning of the code so that you do not need to
%# scroll way down in case you want to change something or if the help is
%# incomplete
options = struct('tol',10^(-15),'f',0.1,'maxdiv',5,'maxord',20,'err_method',1,'domain',1,'coord',0);

%# read the acceptable names
optionNames = fieldnames(options);

%# count arguments
nArgs = length(varargin);
if round(nArgs/2)~=nArgs/2
   error('EXAMPLE needs propertyName/propertyValue pairs')
end

for pair = reshape(varargin,2,[]) %# pair is {propName;propValue}
   inpName = lower(pair{1}); %# make case insensitive

   if any(strmatch(inpName,optionNames))
      %# overwrite options. If you want you can test for the right class here
      %# Also, if you find out that there is an option you keep getting wrong,
      %# you can use "if strcmp(inpName,'problemOption'),testMore,end"-statements
      options.(inpName) = pair{2};
   else
      error('%s is not a recognized parameter name',inpName)
   end
end

err_method = options.('err_method');
maxdiv = options.('maxdiv');
maxord = options.('maxord');
tol = options.('tol');
F = options.('f');
domain = options.('domain');
coord = options.('coord');

cubature_function = cellfun(@str2func,{'cubature_torus_optimized','cubature_sphere_optimized'},'UniformOutput',false);
counter = 0;

ntheta = zeros(maxdiv+1,maxord+2,maxord);
wtheta = zeros(maxdiv+1,maxord+2,maxord);
nphi = zeros(maxdiv+1,maxord+2,maxord);
wphi = zeros(maxdiv+1,maxord+2,maxord);
highntheta = zeros(maxdiv+1,2*maxord+3,maxord);
highnphi = zeros(maxdiv+1,2*maxord+3,maxord);
highwtheta = zeros(maxdiv+1,2*maxord+3,maxord);
highwphi = zeros(maxdiv+1,2*maxord+3,maxord);

% Precalculate nodes and weights
for i=2:maxord
    [ntheta(1:maxdiv+1,1:i+1,i),wtheta(:,1:i+1,i)] = precalculate_gaussian(abs(x(1,2)-x(1,1)) ...
    ,i,maxdiv+1,0);
    [nphi(1:maxdiv+1,1:i+2,i),wphi(:,1:i+2,i)] = precalculate_gaussian(abs(x(2,2)-x(2,1)) ...
    ,i,maxdiv+1,1);
end
% Precalculate also higher order rules for error estimates
if err_method == 2
    for i=2:maxord
        [highntheta(1:maxdiv+1,1:i+2,i),highwtheta(:,1:i+2,i)] = precalculate_gaussian(abs(x(1,2)-x(1,1)) ...
        ,i,maxdiv+1,0,err_method);
        [highnphi(1:maxdiv+1,1:i+3,i),highwphi(:,1:i+3,i)] = precalculate_gaussian(abs(x(2,2)-x(2,1)) ...
        ,i,maxdiv+1,1,err_method);
    end
elseif err_method == 3
    for i=2:maxord
        [highntheta(1:maxdiv+1,1:2*i+1,i),highwtheta(:,1:2*i+1,i)] = precalculate_gaussian(abs(x(1,2)-x(1,1)) ...
        ,i,maxdiv+1,0,err_method);
        [highnphi(1:maxdiv+1,1:2*i+3,i),highwphi(:,1:2*i+3,i)] = precalculate_gaussian(abs(x(2,2)-x(2,1)) ...
        ,i,maxdiv+1,1,err_method);
    end
end

% Init some variables;
queue = [];
done = [];

% Some matrix juggling because otherwise Matlab uses only first return value
x = [x(1,:),x(2,:)];
[res,err,hint,ord,div,reg,evals] = init_region(f,x,0,ntheta,wtheta,nphi,wphi,domain,coord,err_method,highntheta,highwtheta,highnphi,highwphi);
counter = counter + evals;
%printf('Valutazioni della funzione f: %d\n',counter);
% This will be used when we split the region
queue = [queue; res,err,hint,ord,div,reg];

res = queue(end,1);
err = queue(end,2);

% Stop when error goes below threshold in input or we've worked on all regions
while (size(queue,1) > 0 && ...
    err > tol               ...
    )
    % Work on the region with highest error
    [m,index] = max(queue(:,2));
    rect = queue(index,:);
    
    % If the hint is below F, try order adaptive algorithm
    if rect(3) < F

        % If we've reached maximum order, move this region from
        % the original queue to the "done" one
        if rect(4) >= maxord
            queue = [queue(1:index-1,:); queue(index+1:end,:)];
            done = [done; rect];

        % Else, update the info on this region in the queue
        % using higher order formula
        else
            p_res = cubature_function{domain}(f,rect(4)+1, ...
            squeeze(ntheta(rect(5),:,rect(4)+1)),squeeze(wtheta(rect(5),:,rect(4)+1)),squeeze(nphi(rect(5),:,rect(4)+1)),squeeze(wphi(rect(5),:,rect(4)+1)), ...
            rect(6),rect(7),rect(8),rect(9),0,coord);
            counter = counter+1;

            if err_method == 2
                h_res = cubature_function{domain}(f,rect(4)+2, ...
                squeeze(highntheta(rect(5),:,rect(4)+1)),squeeze(highwtheta(rect(5),:,rect(4)+1)),squeeze(highnphi(rect(5),:,rect(4)+1)),squeeze(highwphi(rect(5),:,rect(4)+1)), ...
                rect(6),rect(7),rect(8),rect(9),0,coord);
                counter = counter+1;
            elseif err_method == 3
                h_res = cubature_function{domain}(f,2*(rect(4)+1), ...
                squeeze(highntheta(rect(5),:,rect(4)+1)),squeeze(highwtheta(rect(5),:,rect(4)+1)),squeeze(highnphi(rect(5),:,rect(4)+1)),squeeze(highwphi(rect(5),:,rect(4)+1)), ...
                rect(6),rect(7),rect(8),rect(9),1,coord);
                counter = counter+1;
            else
                h_res = rect(1);
            end
                
            p_err = abs(h_res - p_res);
            if err_method == 2
                p_err = p_err/2;
            end
            rect(4) = rect(4) + 1;
            rect(3) = p_err/rect(2);
            rect(2) = p_err;
            rect(1) = p_res;
            queue = [queue(1:index-1,:); rect; queue(index+1:end,:)];
        end
    % Else, try splitting the domain
    else

        % If we've already split this region enough times,
        % move it to the "done" queue
        if rect(5) >= maxdiv
            queue = [queue(1:index-1,:); queue(index+1:end,:)];
            done = [done; rect];

        % Else, split it and add the new regions to the queue
        else
            xs(:,1) = rect(6:end);
            xs(:,2) = rect(6:end);
            xs(:,3) = rect(6:end);
            xs(:,4) = rect(6:end);
            hdist = (xs(2,1)-xs(1,1))/2;
            vdist = (xs(4,1)-xs(3,1))/2;

            xs(2,1) = xs(2,1) - hdist;
            xs(4,1) = xs(4,1) - vdist;
            xs(1,2) = xs(1,2) + hdist;
            xs(4,2) = xs(4,2) - vdist;
            xs(2,3) = xs(2,3) - hdist;
            xs(3,3) = xs(3,3) + vdist;
            xs(1,4) = xs(1,4) + hdist;
            xs(3,4) = xs(3,4) + vdist;
            part_queue = [queue(1:index-1,:)];
            for j=1:4
                [res,err,hint,ord,div,reg,evals] = init_region(f,xs(:,j)',rect(5),ntheta,wtheta,nphi,wphi,domain,coord,err_method,highntheta,highwtheta,highnphi,highwphi);
                counter = counter + evals;
                part_queue = [part_queue; res, err, hint, ord, div, reg];
            end
            part_queue = [part_queue; queue(index+1:end,:)];
            queue = part_queue;
        end
    end
    % Update new result and error
    % so we can check the "while" condition
    res = 0;
    err = 0;
    if size(queue,2) >= 2
        res  = res + sum(queue(:,1));
        err  = err + sum(queue(:,2));
    end
    if size(done,2) >= 2
        res  = res + sum(done(:,1));
        err  = err + sum(done(:,2));
    end
end

%toc
printf('FINAL_OPT: Valutazioni della funzione f: %d\n',counter);
end

% Helper functions

function [nodes, weights] = precalculate_gaussian(width,deg,maxit,bool_height,method)

% We precalculate nodes and weights for the gaussian formula
% of a fixed degree, so we can reuse them for all the subrectangles
% of a fixed width (must be called separately for width and height)

gaussian = cellfun(@str2func,{'trigauss','antigauss','trigauss_kronrod'},'UniformOutput',false);

if nargin < 5
    method = 1;
end
if nargin < 4
    bool_height = 0;
end
if nargin < 3
    maxit = 6;
end
if nargin < 2
    deg = 64;
end

% Ad-hoc for our 2D formula: the gaussian formula on the second variable
% must have degree of exactness deg +1
if bool_height > 0
    deg = deg + 1;
end
if method == 2
    ndeg = deg + 1;
elseif method == 3
    ndeg = 2*deg;
else
    ndeg = deg;
end

nodes = zeros(maxit,ndeg+1);
weights = zeros(maxit,ndeg+1);

for i=1:maxit
    yy = gaussian{method}(deg,0,width/2^(i-1));
    nodes(i,:) = yy(:,1);

    % We subtract this term because we have not changed trigauss
    % trigauss already centers the nodes, but we want to do it 
    % later when we have the correct subdomain
    nodes(i,:) = nodes(i,:) - (width/2^i);
    weights(i,:) = yy(:,2);
end

end

function [res,err,hint,ord,div,x,evals] = init_region(f,x,div,nt,wt,np,wp,domain,coord,err_method,hnt,hwt,hnp,hwp)

%%%%%
% Generate data structure for new regions
% This data will be the initial queue element for the region
%
% Input: integrand, region, and times it was already split
% Output: integral, error estimate, hint, order, 
% times the region was split and the region itself
%%%%%

cubature_function = cellfun(@str2func,{'cubature_torus_optimized','cubature_sphere_optimized'},'UniformOutput',false);

if nargin < 8
    domain = 1;
end
if nargin < 9
    coord = 0;
end
if nargin < 10
    err_method = 1;
    hnp = 0;
    hnt = 0;
    hwp = 0;
    hwt = 0;
end

init = zeros(4,2);
div = div+1;
evals = 0;

% Similar to order_adaptive strategy - but we only need it up to i=4 
% So that we can have first error estimate and F
for i=2:4
    init(i,1) = cubature_function{domain}(f,i,squeeze(nt(div,:,i)),squeeze(wt(div,:,i)),squeeze(np(div,:,i)),squeeze(wp(div,:,i)),x(1),x(2),x(3),x(4),0,coord);
    evals = evals + 1;

    if err_method == 2
        hres = cubature_function{domain}(f,i+1,squeeze(hnt(div,:,i)),squeeze(hwt(div,:,i)),squeeze(hnp(div,:,i)),squeeze(hwp(div,:,i)),x(1),x(2),x(3),x(4),0,coord);
        init(i,2) = abs(hres - init(i,1))/2;
        evals = evals + 1;
    elseif err_method == 3
        hres = cubature_function{domain}(f,2*i,squeeze(hnt(div,:,i)),squeeze(hwt(div,:,i)),squeeze(hnp(div,:,i)),squeeze(hwp(div,:,i)),x(1),x(2),x(3),x(4),1,coord);
        init(i,2) = abs(hres - init(i,1));
        evals = evals + 1;
    else
        init(i,2) = abs(init(i,1) - init(i-1,1));
    end
end

res = init(4,1);
err = init(4,2);
ord = i;
hint = init(4,2)/init(3,2);
end

function [I] = cubature_torus_optimized(f,n,nt,wt,np,wp,t0,t1,p0,p1,kronrod,coord)

% Helper function
% New version of cubature_2_torus
% optimized to use the precalculated node/weights above

%%%
% Calculating actual node/weights to use
%%%
d = 2;
R = 2;
r = 1;

if nargin < 11
    kronrod = 0;
end
if nargin < 12
    coord = 0;
end

% We go back to same names/dimensions used in cubature_2_torus
% to be more consistent

% Centering the nodes 

yy(1,1:n+1,1) = nt(1:n+1) + ((t0+t1)/2);
yy(2,1:n+2+kronrod,1) = np(1:n+2+kronrod) + ((p0+p1)/2);

yy(1,1:n+1,2) = wt(1:n+1);
yy(2,1:n+2+kronrod,2) = wp(1:n+2+kronrod);

% From this point on, this is cubature_2_torus more or less verbatim

%%%
% Generating node grid
%%%

[A,B]=meshgrid(yy(1,1:n+1,1),yy(2,1:n+2+kronrod,1));

yys(:,1)=A(:);
yys(:,2)=B(:);

[C,D]=meshgrid(yy(1,1:n+1,2),yy(2,1:n+2+kronrod,2));
element = r*(R+ r*cos(yys(:,1)));
yys(:,3)=C(:).*D(:).*element;

thetaw=yys;

%%%
% Calculating cartesian coordinates
%%%

xw=ones(size(thetaw,1),d+2);
xw(:,d+2)=thetaw(:,d+1);
xw(:,1)=cos(thetaw(:,2));
xw(:,2)=sin(thetaw(:,2));
xw(:,1)=xw(:,1).*(R + r*cos(thetaw(:,1)));
xw(:,2)=xw(:,2).*(R + r*cos(thetaw(:,1)));
xw(:,d+1)=r*sin(thetaw(:,1));

%%%
% Calculating integral
%%%

if coord > 0
    fx=feval(f,thetaw(:,1),thetaw(:,2));
    w=thetaw(:,3);
else
    fx=feval(f,xw(:,1),xw(:,2),xw(:,3));
    w=xw(:,4);
end
I=w'*fx;

end

function [I] = cubature_sphere_optimized(f,n,nt,wt,np,wp,t0,t1,p0,p1,kronrod,coord)

% Helper function 
% Optimized version of cubature_2_subsphere_fast

d=2;

if nargin < 11
    kronrod = 0;
end
if nargin < 12
    coord = 0;
end

if (p1-p0==2*pi) && (t0==0) % SPHERICAL CAP.
    %We're not optimizing the cap case

    if mod(n,2)==1
        yy=ones(d,n+1,2);
        yy(1,1:n+1,:)=circle_trigquad(n,p0,p1);
        yy(2,1:n+1,:)=trigauss_abssin(n,t1);

        % In the case of a cap we will consider the volume
        % element directly in trigauss_cap.
        [A,B]=meshgrid(yy(1,1:n+1,1),yy(2,1:((n+1)/2),1));
        yys(:,1)=A(:);
        yys(:,2)=B(:);
        [C,D]=meshgrid(yy(1,1:n+1,2),yy(2,1:((n+1)/2),2));
        yys(:,3)=C(:).*D(:);
    else
        yy=ones(d,n+2,2);
        % We need even number of nodes in [0,2*pi] to have a
        % central simmetry.
        yy(1,1:n+2,:)=circle_trigquad(n+1,p0,p1);
        yy(2,1:n+1,:)=trigauss_abssin(n,t1);

        % In the case of a cap we will consider the volume
        % element directly in trigauss_cap.
        [A,B]=meshgrid(yy(1,1:n+2,1),yy(2,1:floor((n+1)/2),1));
        yys(:,1)=A(:);
        yys(:,2)=B(:);
        [C,D]=meshgrid(yy(1,1:n+2,2),yy(2,1:floor((n+1)/2),2));
        yys(:,3)=C(:).*D(:);
        % Manually add the North Pole.
        yys(end+1,2)=0; yys(end,3)=yy(2,floor((n+1)/2)+1,2)*pi;
    end
    thetaw=yys;

else % SPHERICAL QUADRANGLE.

    % Centering the nodes 
    yy=ones(d,n+2+kronrod,2);
    yy(1,1:n+1,1) = nt(1:n+1) + ((t0+t1)/2);
    yy(2,1:n+2+kronrod,1) = np(1:n+2+kronrod) + ((p0+p1)/2);

    yy(1,1:n+1,2) = wt(1:n+1);
    yy(2,1:n+2+kronrod,2) = wp(1:n+2+kronrod);

    % From this point on, this is cubature_2_subsphere more or less verbatim
    [A,B]=meshgrid(yy(1,1:n+1,1),yy(2,1:n+2+kronrod,1));
    yys(:,1)=A(:);
    yys(:,2)=B(:);
    [C,D]=meshgrid(yy(1,1:n+1,2),yy(2,1:n+2+kronrod,2));
    yys(:,3)=C(:).*D(:).*abs(sin(yys(:,1)));

    thetaw=yys;
end

% Convert all points in cartesian coordinates:
xw=ones(size(thetaw,1),4);
xw(:,1)=cos(thetaw(:,2)).*sin(thetaw(:,1)); % NODES.
xw(:,2)=sin(thetaw(:,2)).*sin(thetaw(:,1));
xw(:,3)=cos(thetaw(:,1));
xw(:,4)=thetaw(:,3); % WEIGHTS.

%%%
% Calculating integral
%%%
if coord > 0
    fx=feval(f,thetaw(:,1),thetaw(:,2));
    w=thetaw(:,3);
else
    fx=feval(f,xw(:,1),xw(:,2),xw(:,3));
    w=xw(:,4);
end
I=w'*fx;
I=w'*fx;

end


function tw=circle_trigquad(n,alpha,beta)

% Generate linearly spaced nodes on the circle
% Needed for spherical cap case above

if nargin == 1
    alpha=0;
    beta=2*pi;
end
N=n+1;
w=(2*pi/N)*ones(N,1);
t=linspace(pi/N,2*pi-pi/N,N); t=alpha+t';

tw=[t w];

end
