%Script confronto numero condizionamento matrice least-squares
%Scritto da Pavan Andrea - 24/07/2022
clear;
clc;


%% dati input
l1 = 1;        %lunghezza lato x [m]
l2 = 1;        %lunghezza lato y [m]
nEdgePoints = 30;       %numero punti in ciascun lato
searchdist = l1/10;      %distanza ricerca punti
Nminsat = 8;        %numero minimo satelliti
lmax = 0.1;        %distanza massima punti


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


%% 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));
        Nsat(i) = length(Psatidx{i});
        searchdisti = searchdisti*1.1;
    end
end

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

%calcolo pesi satelliti
w2 = [];
for i=1:length(P)
    R2 = h{i}.^2+k{i}.^2;
    R2max = max(R2);
    w2{i} = exp(-1*R2/R2max).^2;
    %w2{i} = 1 - 6*(R2/R2max).^2 + 8*(R2/R2max).^3 - 3*(R2/R2max).^4;
end


%% inversione matrici minimi quadrati
T1 = clock;
%invA = [];      %matrici minimi quadrati invertite
condA = zeros(length(P),1);     %numero condizionamento matrici
for i=1:length(P)
    A = [sum(h{i}.^2.*w2{i}), sum(h{i}.*k{i}.*w2{i}), 0.5*sum(h{i}.^3.*w2{i}), 0.5*sum(h{i}.*k{i}.^2.*w2{i}), sum(h{i}.^2.*k{i}.*w2{i});
        sum(h{i}.*k{i}.*w2{i}), sum(k{i}.^2.*w2{i}), 0.5*sum(h{i}.^2.*k{i}.*w2{i}), 0.5*sum(k{i}.^3.*w2{i}), sum(h{i}.*k{i}.^2.*w2{i});
        0.5*sum(h{i}.^3.*w2{i}), 0.5*sum(h{i}.^2.*k{i}.*w2{i}), 0.25*sum(h{i}.^4.*w2{i}), 0.25*sum(h{i}.^2.*k{i}.^2.*w2{i}), 0.5*sum(h{i}.^3.*k{i}.*w2{i});
        0.5*sum(h{i}.*k{i}.^2.*w2{i}), 0.5*sum(k{i}.^3.*w2{i}), 0.25*sum(h{i}.^2.*k{i}.^2.*w2{i}), 0.25*sum(k{i}.^4.*w2{i}), 0.5*sum(h{i}.*k{i}.^3.*w2{i});
        sum(h{i}.^2.*k{i}.*w2{i}), sum(h{i}.*k{i}.^2.*w2{i}), 0.5*sum(h{i}.^3.*k{i}.*w2{i}), 0.5*sum(h{i}.*k{i}.^3.*w2{i}), sum(h{i}.^2.*k{i}.^2.*w2{i})];
    %invA{i} = inv(A);
    condA(i) = cond(A);
end
time1 = etime(clock,T1);



%% grafico numero condizionamento
figure();
scatter(P(:,1),P(:,2),[],log10(condA),'filled');
title('Numero condizionamento log10(cond(A(x,y)))');
xlabel('x (m)');
ylabel('y (m)');
axis equal;
axis square;
colorbar;
hold off;

