%Script prova metodo GFDM
%Equazione Navier-Cauchy - stazionario - 2D
%Predefined connectivity
%Scritto da Pavan Andrea - 04/08/2022
clear;
clc;


%% dati input
l1 = 1;     %lunghezza lato x (m)
l2 = 0.1;       %lunghezza lato y (m)
nEdgePoints1 = 30;       %numero punti lato x
nEdgePoints2 = 10;       %numero punti lato y
lmax = 0.02;        %distanza massima punti
searchdist = lmax/1000;      %distanza ricerca punti
Nminsat = 8;        %numero minimo satelliti
dt = 0.1;       %passo temporale (s)

%proprietà materiale
rho = 2700;     %densità materiale (kg/m3)
E = 73e9;       %modulo Young (Pa)
ni = 0.33;      %coefficiente Poisson
lambda = E*ni/((1+ni)*(1-2*ni));        %primo coefficiente Lamè
mu = E/(2*(1+ni));      %secondo coefficiente Lamè
c = 0;     %coefficiente smorzamento
F = -500;        %forza trazione (N)

%condizioni problema
Fx = @(x,y,t) 0;        %forze di massa
Fy = @(x,y,t) 0;
u0 = @(x,y) 0;      %condizioni iniziali
v0 = @(x,y) 0;
t0 = 0;     %istante iniziale (s)
tf = 5;     %istante finale (s)
ue = @(x,y,t) F*x/(E*l2*1);     %soluzione esatta (m)
ve = @(x,y,t) 0;


%% generazione pointcloud
boundaryNodes = [linspace(0,l1,nEdgePoints1)', 0*ones(nEdgePoints1,1);
    l1+0*ones(nEdgePoints2,1), linspace(0,l2,nEdgePoints2)';
    fliplr(linspace(0,l1,nEdgePoints1))', l2+0*ones(nEdgePoints1,1);
    0*ones(nEdgePoints2,1), fliplr(linspace(0,l2,nEdgePoints2))'];
boundaryNodes = unique(boundaryNodes,'rows','stable');      %punti contorno
P = generate_pointcloud(boundaryNodes,'nextrasteps',0,'lmax',lmax);
P = [boundaryNodes; P];
lbo = length(boundaryNodes);

%normali contorno
nbo = [-1, 0;
        zeros(nEdgePoints1-2,1), -ones(nEdgePoints1-2,1);
        1, 0;
        ones(nEdgePoints2-2,1), zeros(nEdgePoints2-2,1);
        1, 0;
        zeros(nEdgePoints1-2,1), ones(nEdgePoints1-2,1);
        -1, 0;
        -ones(nEdgePoints2-2,1), zeros(nEdgePoints2-2,1)];

%condizioni contorno
idxDirichletu = find(P(1:lbo,1)==0);
idxDirichletv = idxDirichletu;
idxNeumannu = find(P(1:lbo,1)~=0);
idxNeumannv = idxNeumannu;
uD = zeros(length(idxDirichletu),1);
vD = zeros(length(idxDirichletv),1);
uN = zeros(length(idxNeumannu),1);
uN(find(P(1:lbo,1)==l1)) = F/l2;
vN = zeros(length(idxNeumannv),1);


%% popolamento stelle
Psatidx = [];        %elenco punti satellite
Nsat = zeros(length(P),1);      %numero punti satellite
for i=1:length(P)
    %cerco i satelliti del punto i
    searchdisti = searchdist;
    while Nsat(i)<Nminsat
        %Psatidx{i} = find((abs(P(:,1)-P(i,1))<=searchdisti).*(abs(P(:,2)-P(i,2))<=searchdisti).*((P(:,1)-P(i,1)).^2+(P(:,2)-P(i,2)).^2>2*eps));
        Psatidx{i} = find(((P(:,1)-P(i,1)).^2+(P(:,2)-P(i,2)).^2<=searchdisti).*((P(:,1)-P(i,1)).^2+(P(:,2)-P(i,2)).^2>2*eps));
        Nsat(i) = length(Psatidx{i});
        searchdisti = searchdisti*1.1;
    end
end

%calcolo distanze satelliti
x = [];     %distanza x satelliti
y = [];     %distanza y satelliti
for i=1:length(P)
    x{i} = zeros(Nsat(i),1);
    y{i} = zeros(Nsat(i),1);
    for j=1:Nsat(i)
        x{i}(j) = P(Psatidx{i}(j),1)-P(i,1);
        y{i}(j) = P(Psatidx{i}(j),2)-P(i,2);
    end
end

%calcolo pesi satelliti
w2 = [];
for i=1:length(P)
    R2 = x{i}.^2+y{i}.^2;
    R2max = max(R2);
    w2{i} = exp(-1*R2/R2max).^2;
end


%% inversione matrici minimi quadrati
invA = [];     %matrici minimi quadrati invertite
B = [];     %matrice decomposizione termine noto
C = [];     %matrice coefficienti derivate
for i=1:lbo
    %nodo contorno
    %se Dirichlet non vengono usate matrici
    %approccio semplifica l'implementazione nel caso di più variabili
    A = [sum(w2{i}.*(x{i}+nbo(i,1)).^2), sum(w2{i}.*(y{i}+nbo(i,2)).*(x{i}+nbo(i,1))), sum(w2{i}.*x{i}.^2.*(x{i}+nbo(i,1))), sum(w2{i}.*y{i}.^2.*(x{i}+nbo(i,1))), sum(w2{i}.*x{i}.*y{i}.*(x{i}+nbo(i,1)));
        sum(w2{i}.*(y{i}+nbo(i,2)).*(x{i}+nbo(i,1))), sum(w2{i}.*(y{i}+nbo(i,2)).^2), sum(w2{i}.*x{i}.^2.*(y{i}+nbo(i,2))), sum(w2{i}.*y{i}.^2.*(y{i}+nbo(i,2))), sum(w2{i}.*x{i}.*y{i}.*(y{i}+nbo(i,2)));
        sum(w2{i}.*x{i}.^2.*(x{i}+nbo(i,1))), sum(w2{i}.*x{i}.^2.*(y{i}+nbo(i,2))), sum(w2{i}.*x{i}.^4), sum(w2{i}.*y{i}.^2.*x{i}.^2), sum(w2{i}.*x{i}.^3.*y{i});
        sum(w2{i}.*y{i}.^2.*(x{i}+nbo(i,1))), sum(w2{i}.*y{i}.^2.*(y{i}+nbo(i,2))), sum(w2{i}.*y{i}.^2.*x{i}.^2), sum(w2{i}.*y{i}.^4), sum(w2{i}.*x{i}.*y{i}.^3);
        sum(w2{i}.*x{i}.*y{i}.*(x{i}+nbo(i,1))), sum(w2{i}.*x{i}.*y{i}.*(y{i}+nbo(i,2))), sum(w2{i}.*x{i}.^3.*y{i}), sum(w2{i}.*x{i}.*y{i}.^3), sum(w2{i}.*x{i}.^2.*y{i}.^2)];
    invA{i} = inv(A);
    B{i} = zeros(5,2+Nsat(i));
    B{i}(:,1) = [-sum(w2{i}.*(x{i}+nbo(i,1))); -sum(w2{i}.*(y{i}+nbo(i,2))); -sum(w2{i}.*x{i}.^2); -sum(w2{i}.*y{i}.^2); -sum(w2{i}.*x{i}.*y{i})];
    B{i}(1,2:end-1) = w2{i}.*(x{i}+nbo(i,1));
    B{i}(2,2:end-1) = w2{i}.*(y{i}+nbo(i,2));
    B{i}(3,2:end-1) = w2{i}.*x{i}.^2;
    B{i}(4,2:end-1) = w2{i}.*y{i}.^2;
    B{i}(5,2:end-1) = w2{i}.*x{i}.*y{i};
    B{i}(:,end) = -B{i}(:,1);
    C{i} = invA{i}*B{i};
end
for i=lbo+1:length(P)
    %nodo interno
    A = [sum(x{i}.^2.*w2{i}), sum(x{i}.*y{i}.*w2{i}), sum(x{i}.^3.*w2{i}), sum(x{i}.*y{i}.^2.*w2{i}), sum(x{i}.^2.*y{i}.*w2{i});
        sum(x{i}.*y{i}.*w2{i}), sum(y{i}.^2.*w2{i}), sum(x{i}.^2.*y{i}.*w2{i}), sum(y{i}.^3.*w2{i}), sum(x{i}.*y{i}.^2.*w2{i});
        sum(x{i}.^3.*w2{i}), sum(x{i}.^2.*y{i}.*w2{i}), sum(x{i}.^4.*w2{i}), sum(x{i}.^2.*y{i}.^2.*w2{i}), sum(x{i}.^3.*y{i}.*w2{i});
        sum(x{i}.*y{i}.^2.*w2{i}), sum(y{i}.^3.*w2{i}), sum(x{i}.^2.*y{i}.^2.*w2{i}), sum(y{i}.^4.*w2{i}), sum(x{i}.*y{i}.^3.*w2{i});
        sum(x{i}.^2.*y{i}.*w2{i}), sum(x{i}.*y{i}.^2.*w2{i}), sum(x{i}.^3.*y{i}.*w2{i}), sum(x{i}.*y{i}.^3.*w2{i}), sum(x{i}.^2.*y{i}.^2.*w2{i})];
    invA{i} = inv(A);
    B{i} = zeros(5,1+Nsat(i));
    B{i}(:,1) = [-sum(x{i}.*w2{i}); -sum(y{i}.*w2{i}); -sum(x{i}.^2.*w2{i}); -sum(y{i}.^2.*w2{i}); -sum(x{i}.*y{i}.*w2{i})];
    B{i}(1,2:end) = x{i}.*w2{i};
    B{i}(2,2:end) = y{i}.*w2{i};
    B{i}(3,2:end) = x{i}.^2.*w2{i};
    B{i}(4,2:end) = y{i}.^2.*w2{i};
    B{i}(5,2:end) = x{i}.*y{i}.*w2{i};
    C{i} = invA{i}*B{i};
end


%% assemblaggio matrice Eulero Implicito
M = sparse(4*length(P),4*length(P));
for j=1:length(idxDirichletu)
    %nodi contorno Dirichlet variabile u
    i = idxDirichletu(j);
    M(i,i) = 1;
end
for j=1:length(idxNeumannu)
    %nodi contorno Neumann variabile u
    i = idxNeumannu(j);
    M(i,i) = 1 - (dt/rho)*(mu*(2*C{i}(3,1) + 2*C{i}(4,1)) + (mu+lambda)*(2*C{i}(3,1) + 0));
    M(i,i+length(P)) = -(dt/rho)*(mu+lambda)*C{i}(5,1);
    M(i,i+2*length(P)) = c/rho;
    for j=1:Nsat(i)
        M(i,Psatidx{i}(j)) = -(dt/rho)*(mu*(2*C{i}(3,1+j) + 2*C{i}(4,1+j)) + (mu+lambda)*(2*C{i}(3,1+j) + 0));
        M(i,Psatidx{i}(j)+length(P)) = -(dt/rho)*(mu+lambda)*C{i}(5,1+j);
    end
end
for i=1+lbo:length(P)
    %nodi interni variabile u
    M(i,i) = 1 - (dt/rho)*(mu*(2*C{i}(3,1) + 2*C{i}(4,1)) + (mu+lambda)*(2*C{i}(3,1) + 0));
    M(i,i+length(P)) = -(dt/rho)*(mu+lambda)*C{i}(5,1);
    M(i,i+2*length(P)) = c/rho;
    for j=1:Nsat(i)
        M(i,Psatidx{i}(j)) = -(dt/rho)*(mu*(2*C{i}(3,1+j) + 2*C{i}(4,1+j)) + (mu+lambda)*(2*C{i}(3,1+j) + 0));
        M(i,Psatidx{i}(j)+length(P)) = -(dt/rho)*(mu+lambda)*C{i}(5,1+j);
    end
end
for j=1:length(idxDirichletv)
    %nodi contorno Dirichlet variabile v
    i = idxDirichletv(j);
    M(i+length(P),i+length(P)) = 1;
end
for j=1:length(idxNeumannv)
    %nodi contorno Neumann variabile v
    i = idxNeumannv(j);
    M(i+length(P),i+length(P)) = 1 - (dt/rho)*(mu*(2*C{i}(3,1) + 2*C{i}(4,1)) + (mu+lambda)*(0 + 2*C{i}(4,1)));
    M(i+length(P),i) = -(dt/rho)*(mu+lambda)*C{i}(5,1);
    M(i+length(P),i+3*length(P)) = c/rho;
    for j=1:Nsat(i)
        M(i+length(P),Psatidx{i}(j)+length(P)) = -(dt/rho)*(mu*(2*C{i}(3,1+j) + 2*C{i}(4,1+j)) + (mu+lambda)*(0 + 2*C{i}(4,1+j)));
        M(i+length(P),Psatidx{i}(j)) = -(dt/rho)*(mu+lambda)*C{i}(5,1+j);
    end
end
for i=1+lbo:length(P)
    %nodi interni variabile v
    M(i+length(P),i+length(P)) = 1 - (dt/rho)*(mu*(2*C{i}(3,1) + 2*C{i}(4,1)) + (mu+lambda)*(0 + 2*C{i}(4,1)));
    M(i+length(P),i) = -(dt/rho)*(mu+lambda)*C{i}(5,1);
    M(i+length(P),i+3*length(P)) = c/rho;
    for j=1:Nsat(i)
        M(i+length(P),Psatidx{i}(j)+length(P)) = -(dt/rho)*(mu*(2*C{i}(3,1+j) + 2*C{i}(4,1+j)) + (mu+lambda)*(0 + 2*C{i}(4,1+j)));
        M(i+length(P),Psatidx{i}(j)) = -(dt/rho)*(mu+lambda)*C{i}(5,1+j);
    end
end
for i=1+2*length(P):3*length(P)
    %equazione ausiliaria dudt=z1
    M(i,i) = 1;
    M(i,i-2*length(P)) = -dt;
end
for i=1+3*length(P):4*length(P)
    %equazione ausiliaria dvdt=z2
    M(i,i) = 1;
    M(i,i-2*length(P)) = -dt;
end
%spy(M)
%condest(M)


%% evoluzione temporale
t = t0:dt:tf;       %istanti temporali [s]
u = zeros(4*length(P),1);     %soluzione numerica
uprev = u;      %soluzione al passo precedente
erru = [];      %errore assoluto soluzione
maxerru = 0*t;      %errore max soluzione

for l=2:length(t)
    %propagazione metodo Eulero Implicito
    uprev = u;
    p = zeros(4*length(P),1);
    for j=1:length(idxDirichletu)
        %nodi contorno Dirichlet variabile u
        i = idxDirichletu(j);
        p(i) = uD(j);
    end
    for j=1:length(idxNeumannu)
        %nodi contorno Neumann variabile u
        i = idxNeumannu(j);
        p(i) = uprev(i) + dt*0 + (dt/rho)*(mu*(2*C{i}(3,end)*uN(j) + 2*C{i}(4,end)*uN(j)) + (mu+lambda)*(2*C{i}(3,end)*uN(j) + C{i}(5,end)*vN(j)));
    end
    for i=1+lbo:length(P)
        %nodi interni variabile u
        p(i) = uprev(i) + dt*0;
    end
    for j=1:length(idxDirichletv)
        %nodi contorno Dirichlet variabile v
        i = idxDirichletv(j);
        p(i+length(P)) = vD(j);
    end
    for j=1:length(idxNeumannu)
        %nodi contorno Neumann variabile v
        i = idxNeumannv(j);
        p(i) = uprev(i) + dt*0 + (dt/rho)*(mu*(2*C{i}(3,end)*vN(j) + 2*C{i}(4,end)*vN(j)) + (mu+lambda)*(C{i}(5,end)*uN(j) + 2*C{i}(4,end)*vN(j)));
    end
    for i=1+lbo:length(P)
        %nodi interni variabile v
        p(i+length(P)) = uprev(i+length(P)) + dt*0;
    end
    for i=1+2*length(P):3*length(P)
        %equazione ausiliaria dudt=z1
        p(i) = uprev(i);
    end
    for i=1+3*length(P):4*length(P)
        %equazione ausiliaria dvdt=z2
        p(i) = uprev(i);
    end
    u = M\p;

    %calcolo errore e controllo divergenza metodo
    erru{l} = u(1:length(P))./ue(P(:,1),P(:,2),t(l))-1;
    maxerru(l) = max(abs(erru{l}));
%     if max(abs(u))>1e5
%         error('Errore: la soluzione ha superato la soglia 10^5');
%     end

    %grafico evoluzione
    if mod(l,1)==0
        figure(1);
        tiledlayout(1,2);
        nexttile;
        scatter(P(:,1),P(:,2),[],u(1:length(P)),'filled');
        %plot(u(1:length(P)), u(1+length(P):2*length(P)), 'b.');
        title(['Soluzione numerica (t=' num2str(t(l)) 's)']);
        xlabel('x');
        ylabel('y');
        zlabel('Soluzione u(x,y)');
        axis equal;
        colorbar;
        colormap('hot');

        nexttile;
        semilogy(t(1:l),maxerru(1:l),'k-');
        title('Errore soluzione')
        xlabel('Tempo t (s)');
        ylabel('Errore soluzione |u/ue-1|');
    end

end

