function [res,err_est]=set_adaptive_optimized(f,x,varargin)

%%%%%
%
% INPUT:
%   f:    Function to integrate
%   x: Domain of integration (toroidal rectangle)
%   Named Arguments: 
%   tol:    Tolerance
%   maxit: Max number of domain subdivisions
%   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),'maxit',5,'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');
maxit = options.('maxit');
tol = options.('tol');
domain = options.('domain');
coord = options.('coord');

%if nargin < 4
%    maxit = 5;
%end
%
%if nargin < 3
%    tol = 10^(-15);
%end

%tic;

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

ntheta = zeros(maxit+1,maxord+2,maxord);
wtheta = zeros(maxit+1,maxord+2,maxord);
nphi = zeros(maxit+1,maxord+2,maxord);
wphi = zeros(maxit+1,maxord+2,maxord);
highntheta = zeros(maxit+1,2*maxord+3,maxord);
highnphi = zeros(maxit+1,2*maxord+3,maxord);
highwtheta = zeros(maxit+1,2*maxord+3,maxord);
highwphi = zeros(maxit+1,2*maxord+3,maxord);
for i=1:6
    [ntheta(1:maxit+1,1:2^i+1,i),wtheta(:,1:2^i+1,i)] = precalculate_gaussian(abs(x(1,2)-x(1,1)) ...
    ,2^i,maxit+1,0);
    [nphi(1:maxit+1,1:2^i+2,i),wphi(:,1:2^i+2,i)] = precalculate_gaussian(abs(x(2,2)-x(2,1)) ...
    ,2^i,maxit+1,1);
end
if err_method == 2
    for i=1:6
        [highntheta(1:maxit+1,1:2^i+2,i),highwtheta(:,1:2^i+2,i)] = precalculate_gaussian(abs(x(1,2)-x(1,1)) ...
        ,2^i,maxit+1,0,err_method);
        [highnphi(1:maxit+1,1:2^i+3,i),highwphi(:,1:2^i+3,i)] = precalculate_gaussian(abs(x(2,2)-x(2,1)) ...
        ,2^i,maxit+1,1,err_method);
    end
elseif err_method == 3
    for i=1:6
        [highntheta(1:maxit+1,1:2*2^i+1,i),highwtheta(:,1:2*2^i+1,i)] = precalculate_gaussian(abs(x(1,2)-x(1,1)) ...
        ,2^i,maxit+1,0,err_method);
        [highnphi(1:maxit+1,1:2*2^i+3,i),highwphi(:,1:2*2^i+3,i)] = precalculate_gaussian(abs(x(2,2)-x(2,1)) ...
        ,2^i,maxit+1,1,err_method);
    end
end
% Calculating nodes and weights in advance
%for i=1:6
%    [ntheta(1:maxit+1,1:2^i+1,i),wtheta(:,1:2^i+1,i)] = precalculate_gaussian(abs(region(1,2)-region(1,1)) ...
%    ,2^i,maxit+1,0);
%    [nphi(1:maxit+1,1:2^i+2,i),wphi(:,1:2^i+2,i)] = precalculate_gaussian(abs(region(2,2)-region(2,1)) ...
%    ,2^i,maxit+1,1);
%end

format long e;
queue = [];
done = [];

% Computing the integral on the whole region first
[preres,preerr,evals] = order_adaptive_optimized(f,tol,squeeze(ntheta(1,:,:)),squeeze(wtheta(1,:,:)),squeeze(nphi(1,:,:)) ...
    ,squeeze(wphi(1,:,:)),x(1,1),x(1,2),x(2,1),x(2,2),domain,coord,err_method,squeeze(highntheta(1,:,:)),squeeze(highwtheta(1,:,:)),squeeze(highnphi(1,:,:)),squeeze(highwphi(1,:,:)));
x = [x(1,:), x(2,:)];
counter = counter + evals;

queue = [queue; preres, preerr, 0, x];
res = preres;
err_est = queue(end,2);
old_err = err_est;

while (size(queue,1) > 0 && err_est > tol)
   [m, index] = max(queue(:,2));
   rect = queue(index,:);
   flag_break = 0;

    % If we've already recursed maxit times,
    % return this result, don't split anymore
    %rect(3)
    if (rect(3) >= maxit)
            queue = [queue(1:index-1,:); queue(index+1:end,:)];
            done = [done; rect];
            flag_break = 1;
    else

        % Split the region in four subregions,
        % Split along both directions
        xs(:,1) = rect(4:end);
        xs(:,2) = rect(4:end);
        xs(:,3) = rect(4:end);
        xs(:,4) = rect(4:end);
        part_res = [];

        hdist = (xs(2,1)-xs(1,1))/2;
        vdist = (xs(4,1)-xs(3,1))/2;
        if (hdist < 10^(-16) || vdist < 10^(-16))
                queue = [queue(1:index-1,:); queue(index+1:end,:)];
                done = [done; rect];
                flag_break = 1;
        end

        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;

        % Halve the tolerance if it is not too small to do so
        if (tol > 10^(-16))
            tol = tol/2;
        end

        % Actually calculate the partial results on the two parts
        prequeue = [queue(1:index-1,:)];
        for i=1:4
            reg = xs(:,i)';
            % +2 because of strange coincidence; note that in first iteration
            % (i.e. quarters of original domain) rect(3) is still zero, but we 
            % want the second line of ntheta, wtheta... (since first line
            % is for original domain)
            div = rect(3)+2;
            % Using the precalculated data for cubature formula
            [p_res,p_err,p_evals] = order_adaptive_optimized(f,tol,squeeze(ntheta(div,:,:)),squeeze(wtheta(div,:,:)),squeeze(nphi(div,:,:)),squeeze(wphi(div,:,:)),reg(1),reg(2),reg(3),reg(4),domain,coord,err_method,squeeze(highntheta(div,:,:)),squeeze(highwtheta(div,:,:)),squeeze(highnphi(div,:,:)),squeeze(highwphi(div,:,:)));
            prequeue = [prequeue; p_res, p_err, rect(3)+1, reg];
            counter = counter + p_evals;
        end
        prequeue = [prequeue; queue(index+1:end,:)];
        queue = prequeue;
    end

    res = 0;
    err_est = 0;
    if size(queue,2) >= 2
        res  = res + sum(queue(:,1));
        err_est  = err_est + sum(queue(:,2));
    end
    if size(done,2) >= 2
        res  = res + sum(done(:,1));
        err_est  = err_est + sum(done(:,2));
    end
    if flag_break == 0
        old_err = err_est;
%        err_est = abs(res - preres);
    end
    preres = res;

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

end

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

% Helper function
% 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,evals]=order_adaptive_optimized(f,tol,nt,wt,np,wp,t0,t1,p0,p1,domain,coord,err_method,hnt,hwt,hnp,hwp)

%%%%%
%
% INPUT:
%   f:      Function to integrate
%   x:      Region 
%   tol:    Tolerance
%
% OUTPUT:
%   res:    Computed integral
%   err:    Error estimate
% 
%%%%%

if nargin < 11
    domain = 1;
end
if nargin < 12
    coord = 0;
end
if nargin < 13
    err_method = 1;
    hnp = 0;
    hnt = 0;
    hwp = 0;
    hwt = 0;
end
if nargin < 2
    tol = 10^(-15)
end

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

n=2;
res(1)=0;
% Maximum number of iterations
maxexp=6;
evals = 0;

% If we can't reach the required tolerance,
% try doubling the exactness degree of the formula
for i=1:maxexp
    n=2^i;

    % Computing nodes and weights of the formula
    radii = [2,1];
    res(i+1)=cubature_function{domain}(f,n,squeeze(nt(:,i)),squeeze(wt(:,i)),squeeze(np(:,i)),squeeze(wp(:,i)),t0,t1,p0,p1,0,coord);
    evals = evals + 1;

    if err_method == 2
        hres = cubature_function{domain}(f,n,squeeze(hnt(:,i)),squeeze(hwt(:,i)),squeeze(hnp(:,i)),squeeze(hwp(:,i)),t0,t1,p0,p1,0,coord);
        err = abs(hres - res(i+1))/2;
        evals = evals + 1;
    elseif err_method == 3
        hres = cubature_function{domain}(f,n,squeeze(hnt(:,i)),squeeze(hwt(:,i)),squeeze(hnp(:,i)),squeeze(hwp(:,i)),t0,t1,p0,p1,1,coord);
        err = abs(hres - res(i+1));
        evals = evals + 1;
    else
        err = abs(res(i+1) - res(i));
    end
    % Estimate the error using the previous result
    %err=abs(res(i+1)-res(i));

    % We're done if the error is below the threshold in input
    if err < tol
        res=res(end);
        return;
    end
end

res=res(end);
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

% Meshgrid only of the angles
[A,B]=meshgrid(yy(1,1:n+1,1),yy(2,1:n+2+kronrod,1));

%Meshgrid put into a single ""depth-2"" matrix
yys(:,1)=A(:);
yys(:,2)=B(:);

% Meshgrid of the weights
[C,D]=meshgrid(yy(1,1:n+1,2),yy(2,1:n+2+kronrod,2));
% It has the angles in depth 1 and 2, and weights in depth3
element = r*(R+ r*cos(yys(:,1)));
yys(:,3)=C(:).*D(:).*element;

thetaw=yys;
%This was for the if above
%end

xw=ones(size(thetaw,1),d+2);
% Oh, ok, we're copying the weights here... good to know
xw(:,d+2)=thetaw(:,d+1);
% xw(:,1:2) should be x and y
xw(:,1)=cos(thetaw(:,2));
xw(:,2)=sin(thetaw(:,2));
% Multiplying for (R + rcos(theta))
xw(:,1)=xw(:,1).*(R + r*cos(thetaw(:,1)));
xw(:,2)=xw(:,2).*(R + r*cos(thetaw(:,1)));
% xw(:,3) aka z
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)

d=2;

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

if (p1-p0==2*pi) && (t0==0) % SPHERICAL CAP.
    % fprintf('\n \t CAP');

    %We're not optimizing the cap case

    if mod(n,2)==1
        yy=ones(d,n+1,2);
        % yy(1,1:n+1,:)=trigauss_2012(n,x(1,1),x(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,:)=trigauss_2012(n+1,x(1,1),x(1,2));
        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.
    % fprintf('\n \t 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:
% phi=thetaw(:,1), theta=thetaw(:,2), w=thetaw(:,3)
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.

%thetaw=[thetaw(:,2) thetaw(:,1) thetaw(:,3)];

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

%--------------------------------------------------------------------------
% ADDITIONAL ROUTINES.
%--------------------------------------------------------------------------

function tw=circle_trigquad(n,alpha,beta)

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
