% Filtro che considera 2 deviazioni standard in una finestra centrata di
% due settimane (una in avanti e una all'indietro) e mette a zero lo
% spsotamento se è inferiore a tale valore
% E' previsto un secondo filtro forfettario se lo spostamento è minore di
% un determinato valore

function [dX_TLH,dY_TLH,dZ_TLH] = filtro_TLH(dX_TLH,dY_TLH,dZ_TLH,Ndevst,Wdevst,FileName)

fileID = fopen(FileName,'a');
fmt = '%s \r';
text = 'filtro_TLH function started';
fprintf(fileID,fmt,text);

dX_TLH = dX_TLH';
dY_TLH = dY_TLH';
dZ_TLH = dZ_TLH';
calcoloX = dX_TLH;
calcoloY = dY_TLH;
calcoloZ = dZ_TLH;
[r,c]=size(dY_TLH); % riga date, colonna nodi
filtroX = zeros(r,c);
filtroY = zeros(r,c);
filtroZ = zeros(r,c);
finestra = Wdevst;
N_dev_st = Ndevst;
contX = 0;
contY = 0;
contZ = 0;

for j = 1:c % nodi
    for i = 1:r
        if r <= finestra/2 - 0.5
            varX = 0;
            varY = 0;
            varZ = 0;
        else
            if i <= finestra/2 - 0.5
                varX = calcoloX(1:i+(finestra/2-0.5),j);
                varY = calcoloY(1:i+(finestra/2-0.5),j);
                varZ = calcoloZ(1:i+(finestra/2-0.5),j);
            elseif i >= (finestra/2+0.5) && i <= (r-(finestra/2-0.5))
                varX = calcoloX((i-(finestra/2-0.5)):(i+(finestra/2-0.5)),j);
                varY = calcoloY((i-(finestra/2-0.5)):(i+(finestra/2-0.5)),j);
                varZ = calcoloZ((i-(finestra/2-0.5)):(i+(finestra/2-0.5)),j);
            elseif i > (r-(finestra/2-0.5))
                varX = calcoloX((i-(finestra/2-0.5):r),j);
                varY = calcoloY((i-(finestra/2-0.5):r),j);
                varZ = calcoloZ((i-(finestra/2-0.5):r),j);
            end
        end
        % X
        filtroX(i,j) = N_dev_st*std(varX);
        if abs(dX_TLH(i,j)) <= filtroX(i,j)
            if abs(dX_TLH(i,j)) ~=  0
               contX = contX+1;
            end                
            dX_TLH(i,j) = 0;
        end
        % Y
        filtroY(i,j) = N_dev_st*std(varY);
        if abs(dY_TLH(i,j)) <= filtroY(i,j)
            if abs(dY_TLH(i,j)) ~=  0
               contY = contY+1;
            end                
            dY_TLH(i,j) = 0;
        end
        % Z
        filtroZ(i,j) = N_dev_st*std(varZ);
        if abs(dZ_TLH(i,j)) <= filtroZ(i,j)
            if abs(dZ_TLH(i,j)) ~=  0
                contZ = contZ+1;
            end 
            dZ_TLH(i,j) = 0;
        end
    end
end

dX_TLH = dX_TLH';
dY_TLH = dY_TLH';
dZ_TLH = dZ_TLH';

textX = [num2str(contX) ' data filtered along X direction'];
textY = [num2str(contY) ' data filtered along Y direction'];
textD = [num2str(contZ) ' data filtered along Vertical direction'];
fprintf(fileID,fmt,textX);
fprintf(fileID,fmt,textY);
fprintf(fileID,fmt,textD);
text = 'filtro_TLH function closed';
fprintf(fileID,fmt,text);
fclose(fileID);

end