%Script prova generazione pointcloud con algoritmo quadtree unbalanced
%Profilo NACA-2412 su dominio circolare
%Scritto da Pavan Andrea - 12/02/2022
clear;
clc;

%nodi profilo
airfoilNpoints = 100;
airfoil = naca4_generator('2412',airfoilNpoints);

%nodi contorno
% 'O' mesh
boundaryRadius = 20;     %raggio bordo esterno
boundaryNpoints = 50;      %numero punti bordo esterno
boundary(:,1) = 0.25 + boundaryRadius*cos(0:(2*pi/boundaryNpoints):2*pi);       %coordinate punti bordo esterno
boundary(:,2) = boundaryRadius*sin(0:(2*pi/boundaryNpoints):2*pi);
boundary(end,:) = [];

nodes = [airfoil(1:end-1,:); boundary];      %vettore coordinate nodi


%quadtree ricorsivo
nnodes = 1;     %numero punti ammessi per cella
lmax = 5;       %lunghezza massima lato cella
p = [0];        %vettore parent: p(i)=j indica che cella i è figlia della cella j
x = [0.5*(min(nodes(:,1)+max(nodes(:,1))))];        %coordinata x centro cella i
y = [0.5*(min(nodes(:,2)+max(nodes(:,2))))];        %coordinata y centro cella i
l = [max(nodes(:,1))-min(nodes(:,1))];       %lunghezza lato cella i
n = [length(nodes(:,1))];       %numero punti dentro cella i
while max(n)>nnodes
    nold = n;
    for j=1:length(nold)
        if nold(j)>nnodes || l(j)>lmax
            %decompongo nodo j
            p = [p; j; j; j; j];
            l = [l; l(j)/2; l(j)/2; l(j)/2; l(j)/2];
            x = [x; x(j)+l(j)/4; x(j)-l(j)/4; x(j)-l(j)/4; x(j)+l(j)/4];
            y = [y; y(j)+l(j)/4; y(j)+l(j)/4; y(j)-l(j)/4; y(j)-l(j)/4];
            n = [n; 
                sum((nodes(:,1)>=x(j)).*(nodes(:,2)>=y(j)).*(nodes(:,1)<=x(j)+l(j)/2).*(nodes(:,2)<=y(j)+l(j)/2));
                sum((nodes(:,1)<x(j)).*(nodes(:,2)>=y(j)).*(nodes(:,1)>x(j)-l(j)/2).*(nodes(:,2)<=y(j)+l(j)/2));
                sum((nodes(:,1)<x(j)).*(nodes(:,2)<y(j)).*(nodes(:,1)>x(j)-l(j)/2).*(nodes(:,2)>y(j)-l(j)/2));
                sum((nodes(:,1)>=x(j)).*(nodes(:,2)<y(j)).*(nodes(:,1)<=x(j)+l(j)/2).*(nodes(:,2)>y(j)-l(j)/2))];
            l(j) = -1;
            n(j) = -1;
        end
    end
end


%extra-refining ai bordi
nextrasteps = 1;        %numero passi aggiuntivi decomposizione
if nextrasteps>0
    pl = 1:length(p);       %vettore indici foglie
    for j=2:length(p)
        %per prima cosa si trovano le foglie
        pl(p(j)) = NaN;
    end
    pl = pl(~isnan(pl))';
    for j=1:length(pl)
        if n(pl(j))==nnodes
            %decocomposizioni ulteriori
            xnew = linspace(x(pl(j))-(l(pl(j))/2)+(l(pl(j))/(2^(nextrasteps+1))), x(pl(j))+(l(pl(j))/2)-(l(pl(j))/(2^(nextrasteps+1))), 2^nextrasteps);
            ynew = linspace(y(pl(j))-(l(pl(j))/2)+(l(pl(j))/(2^(nextrasteps+1))), y(pl(j))+(l(pl(j))/2)-(l(pl(j))/(2^(nextrasteps+1))), 2^nextrasteps);
            for i=1:length(ynew)
                p = [p; pl(j)+0*xnew(:)];
                x = [x; xnew(:)];
                y = [y; ynew(i)+0*xnew(:)];
                l = [l; l(pl(j))/(2^nextrasteps)+0*xnew(:)];
                n = [n; 0*xnew(:)];
            end
            l(pl(j)) = -1;
            n(pl(j)) = -1;
        end
    end
end


%bilanciamento
pl = 1:length(p);       %vettore indici foglie
for j=2:length(p)
    %per prima cosa si trovano le foglie
    pl(p(j)) = NaN;
end
pl = pl(~isnan(pl))';
x = x(pl);
y = y(pl);
l = l(pl);
n = n(pl);
for j=1:length(pl)
    %trovo celle vicine

    %bordo N
    idxf1 = abs(y(j)+l(j)/2 -(y-l/2))<=2*eps;
    idxf2 = x(j)>x-l/2;
    idxf3 = x(j)<x+l/2;
    idxN = idxf1.*idxf2.*idxf3;

    %bordo W
    idxf1 = abs(x(j)-l(j)/2 -(x+l/2))<=2*eps;
    idxf2 = y(j)>y-l/2;
    idxf3 = y(j)<y+l/2;
    idxW = idxf1.*idxf2.*idxf3;

    %bordo S
    idxf1 = abs(y(j)-l(j)/2 -(y+l/2))<=2*eps;
    idxf2 = x(j)>x-l/2;
    idxf3 = x(j)<x+l/2;
    idxS = idxf1.*idxf2.*idxf3;

    %bordo E
    idxf1 = abs(x(j)+l(j)/2 -(x-l/2))<=2*eps;
    idxf2 = y(j)>y-l/2;
    idxf3 = y(j)<y+l/2;
    idxE = idxf1.*idxf2.*idxf3;

    idx = or(or(or(idxN,idxW),idxS),idxE);
    idx = find(idx);

    %suddivido celle vicine troppo grandi
    for i=1:length(idx)
        k = idx(i);
        if l(k)>1.5*l(j)
            l = [l; l(k)/2; l(k)/2; l(k)/2; l(k)/2];
            x = [x; x(k)+l(k)/4; x(k)-l(k)/4; x(k)-l(k)/4; x(k)+l(k)/4];
            y = [y; y(k)+l(k)/4; y(k)+l(k)/4; y(k)-l(k)/4; y(k)-l(k)/4];
            n = [n; 0; 0; 0; 0];

            l(k) = -1;
            n(k) = -1;
        end
    end
end
nold = [];
x = x(n>=0);
y = y(n>=0);
l = l(n>=0);


%generazione pointcloud
P = [];
for j=1:length(l)
    %verifico quali dei 5 punti della cella j sono interni al dominio

    %punto centrale
    Ptrial = [x(j) y(j)];
    if check_internal_points(Ptrial,airfoil)==false && check_internal_points(Ptrial,boundary)==true
        P = [P; Ptrial];
    end

%     %punto NE
%     Ptrial = [x(j)+l(j)/2 y(j)+l(j)/2];
%     if check_internal_points(Ptrial,airfoil)==false && check_internal_points(Ptrial,boundary)==true
%         P = [P; Ptrial];
%     end
% 
%     %punto NW
%     Ptrial = [x(j)-l(j)/2 y(j)+l(j)/2];
%     if check_internal_points(Ptrial,airfoil)==false && check_internal_points(Ptrial,boundary)==true
%         P = [P; Ptrial];
%     end
% 
%     %punto SW
%     Ptrial = [x(j)-l(j)/2 y(j)-l(j)/2];
%     if check_internal_points(Ptrial,airfoil)==false && check_internal_points(Ptrial,boundary)==true
%         P = [P; Ptrial];
%     end
% 
%     %punto SE
%     Ptrial = [x(j)+l(j)/2 y(j)-l(j)/2];
%     if check_internal_points(Ptrial,airfoil)==false && check_internal_points(Ptrial,boundary)==true
%         P = [P; Ptrial];
%     end
end


%grafico pointcloud
figure(1);
plot(airfoil(:,1),airfoil(:,2),'r.');
hold on;
plot(boundary(:,1),boundary(:,2),'b.');
for j=1:length(P(:,1))
    plot(P(j,1),P(j,2),'k.');
end
title('Grafico pointcloud');
xlabel('x/c');
ylabel('y/c');
axis equal;
axis square;
hold off;


%grafico quadtree
figure(2);
plot(airfoil(:,1),airfoil(:,2),'r.');
hold on;
plot(boundary(:,1),boundary(:,2),'b.');
for j=1:length(l)
    plot([x(j)-l(j)/2 x(j)+l(j)/2], [y(j)+l(j)/2 y(j)+l(j)/2],'b-');
    plot([x(j)-l(j)/2 x(j)-l(j)/2], [y(j)+l(j)/2 y(j)-l(j)/2],'b-');
    plot([x(j)-l(j)/2 x(j)+l(j)/2], [y(j)-l(j)/2 y(j)-l(j)/2],'b-');
    plot([x(j)+l(j)/2 x(j)+l(j)/2], [y(j)+l(j)/2 y(j)-l(j)/2],'b-');
end
title('Grafico suddivisione quadtree');
xlabel('x/c');
ylabel('y/c');
axis equal;
axis square;
hold off;

