% Funzione che ricostrusice i segmenti di pertinenza e la profondità dello
% spostamento relativo.
% Aggiornato il 18 Marzo 2021
function [SpeTL,PsTL,SpeTLHR,PsTLHR,SpeTLH,PsTLH,SpeTLHRH,PsTLHRH,SpeIPL,PsIPL,Inverti,...
    SpeIPLHR,PsIPLHR,SpeHD,PsHD,SpeHDVR,PsHDVR] = schemaSP(yesTL,yesTLHR,yesTLH,yesTLHRH,yesTLHR3D,yesTLHR3DH,...
    yesIPL,yesIPLHR,yesIPLHR3D,yesHD,yesHDVR,NodoTiltLink,NodoTiltLinkHR,NodoTiltLinkHR3D,...
    NodoTiltLinkH,NodoTiltLinkHRH,NodoTiltLinkHR3DH,NodoInPlaceLink,NodoInPlaceLinkHR,...
    NodoInPlaceLinkHR3D,NodoTiltLinkHD,NodoTiltLinkHDVR,Anchor_real,...
    rTL,rTLH,rTLHR,rTLHRH,rIPL,rIPLHR,rHD,rHDVR,catena,IDcentralina,DTcatena,FileName)

% Apro file di testo
fileID = fopen(FileName,'a');
fmt = '%s \r';
text = 'schemaSP function started';
fprintf(fileID,fmt,text);

Shift = Anchor_real;
Nodi = [cell2mat(catena(:,3)) cell2mat(catena(:,2))+Shift cell2mat(catena(:,1))];

% if Anchor_real == 0
%     Last_Node = 0;
% else
%     [r,~] = size(Nodi);
%     % Cerco l'ancora
%     for i = 1:r
%         if Nodi(i,3) == 9
%             break
%         end
%     end
%     Last_Node = Anchor_real - Nodi(i,2) + 0.5;
%     Nodi(:,2) = Nodi(:,2) + Last_Node;
% end

%% Tilt Link V
if  yesTL == 1
% Definizione dei segmenti di pertinenza dei singoli accelerometri
% SpeTL = Segmento di Pertinenza del nodo (posizione i-esima del nodo i-esimo)
% PsTL = Profondità dello spostamento
    NodiTL = -1*cell2mat(NodoTiltLink(:,2:3));
    NodiTL(:,2) = NodiTL(:,2) + Shift;
    % Costruisco il segmento di pertinenza
    spe0 = 0; % segmento di pertinenza dell'ancora
    if rTL > 1
        spe1 = Nodi(1,2)-((NodiTL(1,2) + NodiTL(2,2))/2); % segmento di pertinenza del primo nodo
    else
        spe1 = 1;
    end
    if  rTL>2
        spe2 = zeros(rTL-2,1);
        for i = 2:(rTL-1)      
            media1 = (abs(NodiTL(i,2)) + abs(NodiTL(i-1,2)))/2;
            media2 = (abs(NodiTL(i,2)) + abs(NodiTL(i+1,2)))/2;
            spe2(i-1,1) = media1 - media2;
        end
    else
        spe2 = [];
    end
    if rTL > 1
        spelast = (NodiTL(rTL-1,2)+NodiTL(rTL,2))/2; % segmento di pertinenza del nodo superficiale
        SpeTL = [spe0; spe1; spe2; spelast];
    else
        SpeTL = [spe0; spe1];
    end
    
    % Calcolo la profondità dello spostamento relativo 
    ps0 = (-1)*Nodi(1,2); % profondità dell'ancora
    ps1 = (-1)*Nodi(1,2)+spe1; % la profondità del primo nodo TL è data da 
    % ancora + segmento di pertinenza del primo nodo
    ps2 = zeros(rTL-1,1);
    for j = 2:rTL
        if j == 2
           psj = ps1 + SpeTL(j+1,1); 
           % la profondità a cui il nodo j di tipo Tilt Link rileva lo spostamento è data dalla 
           % somma fra il suo segmento di pertinenza e la profondità a cui rileva lo spostamento
           % il nodo precedente
           psPrec = psj; 
           % mi serve come appoggio per il calcolo della profondità dello spostamento del nodo successivo
        else
            psj = psPrec + SpeTL(j+1,1);
            psPrec = psj;
        end   
        ps2(j-1,1) = psj;
    end
    PsTL = [ps0; ps1; ps2];
    text = 'Segments of relevance and calculation points of Tilt Link V defined correctly';
    fprintf(fileID,fmt,text);
else
    % Non ci sono nodi Tilt Link
    SpeTL = [];
    PsTL = [];
end

%% Tilt Link HD V
if  yesHD == 1
% Definizione dei segmenti di pertinenza dei singoli accelerometri
% SpeTL = Segmento di Pertinenza del nodo (posizione i-esima del nodo i-esimo)
% PsTL = Profondità dello spostamento
    NodiTLHD = -1*cell2mat(NodoTiltLinkHD(:,2:3));
    NodiTLHD(:,2) = NodiTLHD(:,2) + Shift;
    % Costruisco il segmento di pertinenza
    spe0 = 0; % segmento di pertinenza dell'ancora
    if rHD > 1
        spe1 = Nodi(1,2)-((NodiTLHD(1,2) + NodiTLHD(2,2))/2); % segmento di pertinenza del primo nodo
    else
        spe1 = 1;
    end
    if  rHD>2
        spe2 = zeros(rHD-2,1);
        for i = 2:(rHD-1)      
            media1 = (abs(NodiTLHD(i,2)) + abs(NodiTLHD(i-1,2)))/2;
            media2 = (abs(NodiTLHD(i,2)) + abs(NodiTLHD(i+1,2)))/2;
            spe2(i-1,1) = media1 - media2;
        end
    else
        spe2 = [];
    end
    if rHD > 1
        spelast = (NodiTLHD(rHD-1,2)+NodiTLHD(rHD,2))/2; % segmento di pertinenza del nodo superficiale
         SpeHD = [spe0; spe1; spe2; spelast];
    else
        SpeHD = [spe0; spe1];
    end
    
    % Calcolo la profondità dello spostamento relativo 
    ps0 = (-1)*Nodi(1,2); % profondità dell'ancora
    ps1 = (-1)*Nodi(1,2)+spe1; % la profondità del primo nodo TL è data da 
    % ancora + segmento di pertinenza del primo nodo
    ps2 = zeros(rHD-1,1);
    for j = 2:rHD
        if j == 2
            if SpeHD(j+1,1) <= 1
                psj = ps1 + SpeHD(j+1,1);
            else
                psj = cell2mat(NodoTiltLinkHD(j,3))+0.5+Shift;
            end
           % la profondità a cui il nodo j di tipo Tilt Link rileva lo spostamento è data dalla 
           % somma fra il suo segmento di pertinenza e la profondità a cui rileva lo spostamento
           % il nodo precedente
           psPrec = psj; 
           % mi serve come appoggio per il calcolo della profondità dello spostamento del nodo successivo
        else
            if SpeHD(j+1,1) <= 1
                psj = psPrec + SpeHD(j+1,1);
            else
                psj = cell2mat(NodoTiltLinkHD(j,3))+0.5+Shift;
            end
            psPrec = psj;
        end   
        ps2(j-1,1) = psj;
    end
    PsHD = [ps0; ps1; ps2];
    text = 'Segments of relevance and calculation points of Tilt Link HD V defined correctly';
    fprintf(fileID,fmt,text);
    for i = 2:rHD+1
        if SpeHD(i) > 1
            SpeHD(i) = 1;
        end
    end
else
    % Non ci sono nodi Tilt Link
    SpeHD = [];
    PsHD = [];
end

%% Tilt Link HD VR
if  yesHDVR == 1
% Definizione dei segmenti di pertinenza dei singoli accelerometri
% SpeTL = Segmento di Pertinenza del nodo (posizione i-esima del nodo i-esimo)
% PsTL = Profondità dello spostamento
    NodiTLHDVR = -1*cell2mat(NodoTiltLinkHDVR(:,2:3));
    NodiTLHDVR(:,2) = NodiTLHDVR(:,2) + Shift;
    % Costruisco il segmento di pertinenza
    spe0 = 0; % segmento di pertinenza dell'ancora
    if rHDVR > 1
        spe1 = Nodi(1,2)-((NodiTLHDVR(1,2) + NodiTLHDVR(2,2))/2); % segmento di pertinenza del primo nodo
    else
        spe1 = 1;
    end
    if  rHDVR>2
        spe2 = zeros(rHDVR-2,1);
        for i = 2:(rHDVR-1)      
            media1 = (abs(NodiTLHDVR(i,2)) + abs(NodiTLHDVR(i-1,2)))/2;
            media2 = (abs(NodiTLHDVR(i,2)) + abs(NodiTLHDVR(i+1,2)))/2;
            spe2(i-1,1) = media1 - media2;
        end
    else
        spe2 = [];
    end
    if rHDVR > 1
        spelast = (NodiTLHDVR(rHDVR-1,2)+NodiTLHDVR(rHDVR,2))/2; % segmento di pertinenza del nodo superficiale
         SpeHDVR = [spe0; spe1; spe2; spelast];
    else
        SpeHDVR = [spe0; spe1];
    end
    
    % Calcolo la profondità dello spostamento relativo 
    ps0 = (-1)*Nodi(1,2); % profondità dell'ancora
    ps1 = (-1)*Nodi(1,2)+spe1; % la profondità del primo nodo TL è data da 
    % ancora + segmento di pertinenza del primo nodo
    ps2 = zeros(rHDVR-1,1);
    for j = 2:rHDVR
        if j == 2
            if SpeHDVR(j+1,1) <= 1
                psj = ps1 + SpeHDVR(j+1,1);
            else
                psj = cell2mat(NodoTiltLinkHDVR(j,3))+0.5+Shift;
            end
           % la profondità a cui il nodo j di tipo Tilt Link rileva lo spostamento è data dalla 
           % somma fra il suo segmento di pertinenza e la profondità a cui rileva lo spostamento
           % il nodo precedente
           psPrec = psj; 
           % mi serve come appoggio per il calcolo della profondità dello spostamento del nodo successivo
        else
            if SpeHDVR(j+1,1) <= 1
                psj = psPrec + SpeHDVR(j+1,1);
            else
                psj = cell2mat(NodoTiltLinkHDVR(j,3))+0.5+Shift;
            end
            psPrec = psj;
        end   
        ps2(j-1,1) = psj;
    end
    PsHDVR = [ps0; ps1; ps2];
    text = 'Segments of relevance and calculation points of Tilt Link HD VR defined correctly';
    fprintf(fileID,fmt,text);
    for i = 2:rHDVR+1
        if SpeHDVR(i) > 1
            SpeHDVR(i) = 1;
        end
    end
else
    % Non ci sono nodi Tilt Link VR
    SpeHDVR = [];
    PsHDVR = [];
end

%% Tilt Link HR V
if  yesTLHR == 1
    % Definizione dei segmenti di pertinenza delle ampolle
    % PsTLHR = Profondità dello spostamento
    NodiTLHR = -1*cell2mat(NodoTiltLinkHR(:,2:3));  
    NodiTLHR(:,2) = NodiTLHR(:,2) + Shift;
    % Costruisco il segmento di pertinenza
    spe0 = 0; % segmento di pertinenza dell'ancora
    if rTLHR == 1 % c'è una sola ampolla
       SpeTLHR = [0; 0.5]; % lo impongo di default
    else
        spe1 = Nodi(1,2)-((NodiTLHR(1,2) + NodiTLHR(2,2))/2); % segmento di pertinenza del primo nodo
        if rTLHR>2
            spe2 = zeros(rTLHR-2,1);
            for i = 2:(rTLHR-1)      
                media1 = (abs(NodiTLHR(i,2)) + abs(NodiTLHR(i-1,2)))/2;
                media2 = (abs(NodiTLHR(i,2)) + abs(NodiTLHR(i+1,2)))/2;
                spei = media1 - media2;
                spe2(i-1,1) = spei;
            end
        else
            spe2 = [];
        end
        spelast = (NodiTLHR(rTLHR-1,2)+NodiTLHR(rTLHR,2))/2; % segmento di pertinenza del nodo superficiale
        SpeTLHR = [spe0; spe1; spe2; spelast];    
    end

    % Calcolo la profondità dello spostamento relativo 
    ps0 = (-1)*Nodi(1,2); % profondità dell'ancora
    if rTLHR == 1 % c'è una sola ampolla
       PsTLHR = [ps0; -1*NodiTLHR(1,2)];
    else
        ps1 = (-1)*Nodi(1,2)+spe1; % la profondità del primo nodo TLHR è data 
                                   % da ancora + segmento di pertinenza del primo nodo
        ps2 = zeros(rTLHR-1,1);
        for j = 2:rTLHR
            if  j == 2
                psj = ps1 + SpeTLHR(j+1,1); 
                % la profondità a cui il nodo j di tipo Tilt Link HR rileva lo 
                % spostamento è data dalla somma fra il suo segmento di 
                % pertinenza e la profondità a cui rileva lo spostamento 
                % il nodo precedente
                psPrec = psj; % mi serve come appoggio per il calcolo della 
                        % profondità dello spostamento del nodo successivo
            else
                psj = psPrec + SpeTLHR(j+1,1);
                psPrec = psj;
            end   
            ps2(j-1,1) = psj;
        end
        PsTLHR = [ps0; ps1; ps2];
    end
    
    if  yesTL == 1 % Se ci sono Tilt Link, il segmento di pertinenza 
        % dell'ampolla lo scrivo considerando i nodi Tilt Link sopra e
        % sotto (se sono presenti)
        salta = zeros(rTLHR,1);
        esco = 0; % entra o meno nel ciclo successivo

        if yesTLHR3D == 1
           [dim,~] = size(NodoTiltLinkHR3D);
           if rTLHR == dim % vuol dire che tutti i nodi sono dopppi
              esco = 1; % non entro nel ciclo successivo
           end
        end
        
        if  esco == 0
            NodiTLdown = zeros(rTLHR,1);
            NodiTLup = zeros(rTLHR,1);
            ProfTLHR = zeros(rTLHR*3,1);
            for i = 1:rTLHR
                prof = NodiTLHR(i,2);
                % cerco il nodo sotto all'Ampolla
                B = NodiTL(:,2) > prof; % uso > perchè il segno della profondità è +
                A = NodiTL(B,2);
                if  i == 1 % guardo se il primo nodo è Ampolla o Mems
                    C = isempty(A);
                    if C == 1 % il primo nodo è Ampolla
                        salta(i) = 1;
                    else
                        salta(i) = 0;
                    end
                end
                NodoTLdown = A(end); % nodo Tilt Link sotto all'Ampolla
                
                % cerco il nodo dopo l'Ampolla
                B = NodiTL(:,2) < prof;
                A = NodiTL(B,2);
                CC = isempty(A);
                if C == 1 % Manca il nodo Sotto
                    salta(i) = 1;
                else
                    if CC == 1 % ovvero se Tilt Link e Tilt Link HR sono nello
                          % stesso nodo doppio, ignoro il passaggio (manca
                          % il nodo sopra)
                       salta(i) = 1;
                    else
                        salta(i) = 0;
                        NodoTLup = A(1);
                        NodiTLdown(i,1) = NodoTLdown;
                        NodiTLup(i,1) = NodoTLup; % Nodo Tilt Link sopra all'ampolla
                    end
                end
            end

            spe0 = 0; % segmento di pertinenza dell'ancora
            if salta(1) == 1
               spe1 = Nodi(1,2)-NodiTLHR(1,2); % segmento di pertinenza del primo nodo
               ii = 2;
            else
                spe1 = Nodi(1,2)-NodiTLdown(1,1); % segmento di pertinenza del 
                % primo nodo. N.B.: Per funzionare correttamente, il primo nodo 
                % dopo l'ancora deve per forza essere un Tilt Link!
                ii = 1;
            end
            spe2 = zeros(rTLHR,1);
            for i = ii:rTLHR
                spei = (NodiTLdown(i,1)-NodiTLup(i,1))/2;
                spe2(i,1) = spei;
            end
            SpeTLHR = [spe0; spe1; spe2];

            % Calcolo profondità dello spostamento relativo
            m = 1;
            
            for i = 1:rTLHR
                if salta(i) == 1
                   if i == 1 % considero l'ancora
                        a = (Nodi(i,2)+NodiTLHR(i,2))/2;
                        ProfTLHR = PsTLHR;
                    else
                        a = (NodiTLHR(i-1,2)+NodiTLHR(i,2))/2;
                    end 
                    if i == rTLHR % ultimo nodo
                       if salta(i) == 1    
                          b = NodiTLHR(i,2);
                          c = NodiTLHR(i,2)+0.25; 
                       else    
                            b = (NodiTLHR(i,2)+NodiTLup(i))/2;
                            c = NodiTLup(i)-(NodiTLHR(i,2)-NodiTLup(i))/2;
                       end
                    else
                       if salta(i+1) == 1
                            b = (NodiTLHR(i,2)+NodiTLHR(i+1,2))/2;
                            c = (NodiTLHR(i+1,2)-(NodiTLHR(i,2)-NodiTLHR(i+1,2)))/2;
                        else
                            b = (NodiTLHR(i,2)+NodiTLup(i))/2;
                            c = NodiTLup(i)-(NodiTLHR(i,2)-NodiTLup(i))/2;
                       end
                    end
                else
                    a = (NodiTLdown(i)+NodiTLHR(i,2))/2;
                    b = (NodiTLHR(i,2)+NodiTLup(i))/2;
                    c = (NodiTLup(i)-(NodiTLHR(i,2)-NodiTLup(i))/2);
                end
                ProfTLHR(m,1) = a;
                ProfTLHR(m+1,1) = b;
                ProfTLHR(m+2,1) = c;
                m = m+3;
            end
        end
    end  
   text = 'Segments of relevance and calculation points of Tilt Link HR V defined correctly';
   fprintf(fileID,fmt,text);
else
    % Non ci sono nodi Tilt Link HR
    PsTLHR = [];
    SpeTLHR = [];
end

%% Tilt Link HR3D
% % CONTROLLA E CORREGGI. lA CORREZIONE FATTA QUI VA PURE SU TL E HR
if  yesTLHR3D == 1
    NodoTLHR3D = zeros(rTL,1);
    t = 1;
    for i=1:rTL
        for j=1:rTLHR
            if NodiTL(i,2)==NodiTLHR(j,2)
               NodoTLHR3D(t,1)=NodiTL(i,2);
               SpeTLHR(j+1,1) = SpeTL(i+1,1);
               PsTLHR(j+1,1) = PsTL(i+1,1);
               % il segmento di pertinenza dell'ampolla è identico al SP del corrispondente Tilt Link V
               t=t+1;
            end
        end
    end
end

%% Tilt Link H
if  yesTLH == 1
% Definizione dei segmenti di pertinenza dei singoli accelerometri
% SpeTLH = Segmento di Pertinenza del nodo (posizione i-esima del nodo i-esimo)
% PsTLH = Posizione dello spostamento
    Nodi_H = Nodi;
    NodiTLH = cell2mat(NodoTiltLinkH(:,2:3));
    
    % Costruisco il segmento di pertinenza
    spe0 = 0; % segmento di pertinenza dell'ancora
    spe1 = -1*(Nodi_H(1,2)-((NodiTLH(1,2) + NodiTLH(2,2))/2)); % segmento di pertinenza del primo nodo, lo considero +
    if  rTLH>2
        spe2 = zeros(rTLH-2,1);
        for i = 2:(rTLH-1)      
            media1 = (abs(NodiTLH(i,2)) + abs(NodiTLH(i-1,2)))/2;
            media2 = (abs(NodiTLH(i,2)) + abs(NodiTLH(i+1,2)))/2;
            spei = media1 - media2;
            spe2(i-1,1) = -1*(spei); % li considero positivi
        end
    else
        spe2 = [];
    end
    % segmento di pertinenza dell'ultimo nodo, lo considero centrando il
    % nodo rispetto al nodo sotto e a un ipotetico nodo sopra equidistante
    spelast = (NodiTLH(end,2)+(NodiTLH(end,2)-NodiTLH(rTLH-1,2))/2-((NodiTLH(rTLH-1,2)+NodiTLH(rTLH,2)))/2);
    SpeTLH = [spe0; spe1; spe2; spelast]; % raccolgo i segmenti di pertinenza
    
    % Calcolo la posizione dello spostamento relativo 
    ps0 = Nodi_H(1,2); % posizione dell'ancora
    if spe1 <= 1
        ps1 = Nodi_H(1,2)+spe1; % la posizione del primo nodo TLH è data da
    else
        ps1 = cell2mat(NodoTiltLinkH(1,3))+0.5;
    end
    % ancora + segmento di pertinenza del primo nodo
    ps2 = zeros(rTLH-1,1);
    for j = 2:rTLH
        if j == 2
           if SpeTLH(j+1,1) <= 1
                psj = ps1 + SpeTLH(j+1,1);
            else
                psj = cell2mat(NodoTiltLinkH(j,3))+0.5;
            end
           % la posizione a cui il nodo j di tipo Tilt Link H rileva lo spostamento è data dalla 
           % somma fra il suo segmento di pertinenza e la posizione a cui rileva lo spostamento
           % il nodo precedente
           psPrec = psj; 
           % mi serve come appoggio per il calcolo della posizione dello spostamento del nodo successivo
        else
            if SpeTLH(j+1,1) <= 1
                psj = psPrec + SpeTLH(j+1,1);
            else
                psj = cell2mat(NodoTiltLinkH(j,3))+0.5;
            end
            psPrec = psj;
        end   
        ps2(j-1,1) = psj;
    end
    PsTLH = [ps0; ps1; ps2];
    text = 'Segments of relevance and calculation points of Tilt Link H defined correctly';
    fprintf(fileID,fmt,text);
    for i = 2:rTLH+1
        if SpeTLH(i) > 1
           SpeTLH(i) = 1;
        end
    end
else
    % Non ci sono nodi Tilt Link H
    SpeTLH = [];
    PsTLH = [];
end

%% Tilt Link HR H
if  yesTLHRH == 1
    % Definizione dei segmenti di pertinenza delle ampolle
    % PsTLHRH = posizione dello spostamento
    Nodi_HRH = Nodi;
    NodiTLHRH = cell2mat(NodoTiltLinkHRH(:,2:3));

    % Costruisco il segmento di pertinenza
    spe0 = 0; % segmento di pertinenza dell'ancora
    spe1 = -1*(Nodi_HRH(1,2)-((NodiTLHRH(1,2)+NodiTLHRH(2,2))/2)); % segmento di pertinenza del primo nodo
    spe2 = zeros(rTLHRH-2,1);
    if rTLHRH>2
        for i = 2:(rTLHRH-1)      
            media1 = (abs(NodiTLHRH(i,2)) + abs(NodiTLHRH(i-1,2)))/2;
            media2 = (abs(NodiTLHRH(i,2)) + abs(NodiTLHRH(i+1,2)))/2;
            spei = media1 - media2;
            spe2(i-1,1) = -1*(spei); % li considero positivi
        end
    else
        spe2 = [];
    end
     % segmento di pertinenza dell'ultimo nodo, lo considero centrando il
    % nodo rispetto al nodo sotto e a un ipotetico nodo sopra equidistante
    spelast = (NodiTLHRH(end,2)+(NodiTLHRH(end,2)-NodiTLHRH(rTLH-1,2))/2-((NodiTLHRH(rTLH-1,2)+NodiTLHRH(rTLH,2)))/2);
    % raccolgo i segmenti di pertinenza
    SpeTLHRH = [spe0; spe1; spe2; spelast];

    % Calcolo la posizione dello spostamento relativo 
    ps0 = Nodi_HRH(1,2); % posizione dell'ancora
    if spe1 <= 1
        ps1 = Nodi_HRH(1,2)+spe1; % la posizione del primo nodo TLHRH è data da ancora + segmento di pertinenza del primo nodo
    else
        ps1 = cell2mat(NodoTiltLinkHRH(1,3))+0.5;
    end
    ps2 = zeros(rTLHRH-1,1);
    for j = 2:rTLHRH
        if  j == 2
            if SpeTLHRH(j+1,1) <= 1
                psj = ps1 + SpeTLHRH(j+1,1);
            else
                psj = cell2mat(NodoTiltLinkHRH(j,3))+0.5;
            end
            % la posizione a cui il nodo j di tipo Tilt Link HR H rileva lo 
            % spostamento è data dalla somma fra il suo segmento di 
            % pertinenza e la posizione a cui rileva lo spostamento 
            % il nodo precedente
            psPrec = psj; % mi serve come appoggio per il calcolo della 
                    % posizione dello spostamento del nodo successivo
        else
            if SpeTLHRH(j+1,1) <= 1
                psj = psPrec + SpeTLHRH(j+1,1);
            else
                psj = cell2mat(NodoTiltLinkHRH(j,3))+0.5;
            end
            psPrec = psj;
        end   
        ps2(j-1,1) = psj;
    end
    PsTLHRH = [ps0; ps1; ps2];
    for i = 2:rTLHRH+1
        if SpeTLHRH(i) > 1
            SpeTLHRH(i) = 1;
        end
    end

    if  yesTLH == 1 % Se ci sono Tilt Link H, il segmento di pertinenza 
        % dell'ampolla lo scrivo considerando i nodi Tilt Link H prima e
        % dopo (se sono presenti), a meno che questi non siano Tilt Link
        % HR3D H
        salta = zeros(rTLHRH,1);
        esco = 0; % entra o meno nel ciclo successivo
        if yesTLHR3DH == 1
           [dim,~] = size(NodoTiltLinkHR3DH);
           if rTLHRH == dim % vuol dire che tutti i nodi sono dopppi
              esco = 1; % esco dal ciclo successivo
           end
        end

        if  esco == 0
            NodiTLHdown = zeros(rTLHRH,1);
            NodiTLHup = zeros(rTLHRH,1);
            ProfTLHRH = zeros(rTLHRH*3,1);
            for i = 1:rTLHRH
                prof = NodiTLHRH(i,2);
                % cerco il nodo prima dell'Ampolla
                B = NodiTLH(:,2) < prof; 
                A = NodiTLH(B,2);
                if  i == 1 % guardo se il primo nodo è Ampolla o Tilt Link
                    C = isempty(A);
                    if C == 1 % il primo nodo è Ampolla
                       salta(i) = 1;
                    else
                        salta(i) = 0;
                    end
                end

                NodoTLHdown = A(end); % nodo Tilt Link H prima dell'Ampolla

                % cerco il nodo dopo l'Ampolla
                B = NodiTLH(:,2) > prof;
                A = NodiTLH(B,2);
                CC = isempty(A);
                if C == 1 % Manca il nodo dopo
                   salta(i) = 1;
                else
                    if CC == 1 % ovvero se Tilt Link e Tilt Link HR sono nello
                          % stesso nodo doppio, ignoro il passaggio (manca
                          % il nodo sopra)
                       salta(i) = 1;
                    else
                        salta(i) = 0;
                        NodoTLHup = A(1);
                        NodiTLHdown(i,1) = NodoTLHdown;
                        NodiTLHup(i,1) = NodoTLHup; % Nodo Tilt Link H dopo l'ampolla
                    end
                end
            end
            
            spe0 = 0; % segmento di pertinenza dell'ancora
            if  salta(1) == 1
                spe1 = Nodi(1,2)-NodiTLHRH(1,2); % segmento di pertinenza del primo nodo
                ii = 2;
            else
                spe1 = Nodi(1,2)-NodiTLHdown(1,1); % segmento di pertinenza del 
                % primo nodo. N.B.: Per funzionare correttamente, il primo nodo 
                % dopo l'ancora deve per forza essere un Tilt Link H!
                ii = 1;
            end
            spe2 = zeros(rTLHRH,1);
            for i = ii:rTLHRH
                spei = (NodiTLHdown(i,1)-NodiTLHup(i,1))/2;
                spe2(i,1) = spei;
            end
            
            SpeTLHRH = [spe0; spe1; spe2];
            % Calcolo posizione dello spostamento relativo
            m = 1;

            for i = 1:rTLHRH
                if salta(i) == 1
                    if i == 1 % considero l'ancora
                        a = -1*((Nodi_HRH(i,2)+NodiTLHRH(i,2))/2);
                        ProfTLHRH = PsTLHRH;
                    else
                        a = -1*((NodiTLHRH(i-1,2)+NodiTLHRH(i,2))/2);
                    end
                    if i == rTLHRH % ultimo nodo
                        if salta(i) == 1
                            b = -1*(NodiTLHRH(i,2));
                            c = -1*(NodiTLHRH(i,2)+0.25);
                        else
                            b = -1*((NodiTLHRH(i,2)+NodiTLHup(i))/2);
                            c = -1*(NodiTLHup(i)-(NodiTLHRH(i,2)-NodiTLHup(i))/2);
                        end
                    else    
                        if salta(i+1) == 1
                            b = -1*((NodiTLHRH(i,2)+NodiTLHRH(i+1,2))/2);
                            c = -1*(NodiTLHRH(i+1,2)-(NodiTLHRH(i,2)-NodiTLHRH(i+1,2))/2);
                        else
                            b = -1*((NodiTLHRH(i,2)+NodiTLHup(i))/2);
                            c = -1*(NodiTLHup(i)-(NodiTLHRH(i,2)-NodiTLHup(i))/2);
                        end
                    end
                else
                    a = -1*((NodiTLHdown(i)+NodiTLHRH(i,2))/2);
                    b = -1*((NodiTLHRH(i,2)+NodiTLHup(i))/2);
                    c = -1*(NodiTLHup(i)-(NodiTLHRH(i,2)-NodiTLHup(i))/2);
                end
                ProfTLHRH(m,1) = a;
                ProfTLHRH(m+1,1) = b;
                ProfTLHRH(m+2,1) = c;
                m = m+3;
            end        
        end
    text = 'Segments of relevance and calculation points of Tilt Link HR H defined correctly';
    fprintf(fileID,fmt,text);
    end    
else
    % Non ci sono nodi Tilt Link HR H
    PsTLHRH = [];
    SpeTLHRH = [];
end

%% Tilt Link HR3D H
if  yesTLHR3DH == 1
    NodoTLHR3DH = zeros(rTLH,1);
    t = 1;
    for i=1:rTLH
        for j=1:rTLHRH
            if NodiTLH(i,2)==NodiTLHRH(j,2)
               NodoTLHR3DH(t,1)=NodiTLH(i,2);
               SpeTLHRH(j+1,1) = SpeTLH(i+1,1); 
               PsTLHRH(j+1,1) = PsTLH(i+1,1);
               t=t+1;
            end
        end
    end
end    

%% In Place Link
if  yesIPL == 1
    NodiIPL = -1*cell2mat(NodoInPlaceLink(:,2:3));
    NodiIPL(:,2) = NodiIPL(:,2) + Shift;
    % Controllo quale sia il numero del nodo più profondo per invertire
    % eventualmente le matrici
    Nodo1 = find(cell2mat(NodoInPlaceLink(:,2))==1);
    Prof1 = cell2mat(NodoInPlaceLink(Nodo1,3));
    Nodo2 = find(cell2mat(NodoInPlaceLink(:,2))==2);
    Prof2 = cell2mat(NodoInPlaceLink(Nodo2,3));
    if Prof2<Prof1
        Inverti=1;
    else
        Inverti=0;
    end
    % Costruisco il segmento di pertinenza
    spe0 = 0; % segmento di pertinenza dell'ancora
    if Inverti == 0
        spe1 = Nodi(1,2)-((NodiIPL(1,2) + NodiIPL(2,2))/2); % segmento di pertinenza del primo nodo
    elseif Inverti == 1
        spe1 = Nodi(end,2)-((NodiIPL(end,2) + NodiIPL(end-1,2))/2); % segmento di pertinenza del primo nodo
    end
    if  rIPL>2
        spe2 = zeros(rIPL-2,1);
        for i = 2:(rIPL-1)  
            if Inverti == 0
                media1 = (abs(NodiIPL(i,2)) + abs(NodiIPL(i-1,2)))/2;
                media2 = (abs(NodiIPL(i,2)) + abs(NodiIPL(i+1,2)))/2;
            elseif Inverti == 1
                media1 = (abs(NodiIPL(rIPL-i+1,2)) + abs(NodiIPL(rIPL,2)))/2;
                media2 = (abs(NodiIPL(rIPL-i+1,2)) + abs(NodiIPL(rIPL-i,2)))/2;
            end
            spe2(i-1,1) = media1 - media2;
        end
    else
        spe2 = [];
    end
    if Inverti == 0
        spelast = (NodiIPL(rIPL-1,2)+NodiIPL(rIPL,2))/2; % segmento di pertinenza del nodo superficiale
    elseif Inverti == 1
        spelast = (NodiIPL(2,2)+NodiIPL(1,2))/2; % segmento di pertinenza del nodo superficiale
    end
    SpeIPL = [spe0; spe1; spe2; spelast];
    
    % Calcolo la profondità dello spostamento relativo
    if Inverti == 0
        ps0 = (-1)*Nodi(1,2); % profondità dell'ancora
    elseif Inverti == 1
        ps0 = (-1)*Nodi(end,2); % profondità dell'ancora
    end
    if spe1 <= 1
        ps1 = (-1)*Nodi(1,2)+spe1; % la profondità del primo nodo IPL è data da
    else
        ps1 = cell2mat(NodoInPlaceLink(1,3))+0.5;
    end
    % ancora + segmento di pertinenza del primo nodo
    ps2 = zeros(rIPL-1,1);
    for j = 2:rIPL
        if j == 2
            if SpeIPL(j+1,1) <= 1
                psj = ps1 + SpeIPL(j+1,1);
            else
                psj = cell2mat(NodoInPlaceLink(j,3))+0.5+Shift;
            end

           % la profondità a cui il nodo j di tipo In Place Link rileva lo spostamento è data dalla 
           % somma fra il suo segmento di pertinenza e la profondità a cui rileva lo spostamento
           % il nodo precedente
           psPrec = psj; 
           % mi serve come appoggio per il calcolo della profondità dello spostamento del nodo successivo
        else
            if strcmp(IDcentralina,'ID0070') == 1 && strcmp(DTcatena,'DT0111 IPI') == 1 || ...
                strcmp(IDcentralina,'ID0071') == 1 && strcmp(DTcatena,'DT0112 IPI') == 1 || ...
                strcmp(IDcentralina,'ID0072') == 1 && strcmp(DTcatena,'DT0113 IPI') == 1 || ...
                strcmp(IDcentralina,'ID0073') == 1 && strcmp(DTcatena,'DT0114 IPI') == 1
                if SpeIPL(j+1,1) <= 0.5
                    psj = psPrec + SpeIPL(j+1,1);
                else
                    psj = cell2mat(NodoInPlaceLink(j,3))+0.25+Shift;
                end
            else
                if SpeIPL(j+1,1) <= 1
                    psj = psPrec + SpeIPL(j+1,1);
                else
                    psj = cell2mat(NodoInPlaceLink(j,3))+0.5+Shift;
                end
            end

            psPrec = psj;
        end   
        ps2(j-1,1) = psj;
    end
    PsIPL = [ps0; ps1; ps2];
    text = 'Segments of relevance and calculation points of In Place Link defined correctly';
    fprintf(fileID,fmt,text);
    for i = 2:rIPL+1
        if SpeIPL(i) > 1
           SpeIPL(i) = 1;
        end
        if strcmp(IDcentralina,'ID0070') == 1 && strcmp(DTcatena,'DT0111 IPI') == 1 || ...
                strcmp(IDcentralina,'ID0071') == 1 && strcmp(DTcatena,'DT0112 IPI') == 1 || ...
                strcmp(IDcentralina,'ID0072') == 1 && strcmp(DTcatena,'DT0113 IPI') == 1 || ...
                strcmp(IDcentralina,'ID0073') == 1 && strcmp(DTcatena,'DT0114 IPI') == 1
            if SpeIPL(i) > 0.5
                SpeIPL(i) = 0.5;
            end
        end
    end
else
    % Non ci sono nodi In Place Link
    SpeIPL = [];
    PsIPL = [];
    Inverti = [];
end

%% In Place Link HR
if  yesIPLHR == 1
    % Definizione dei segmenti di pertinenza delle ampolle
    % PsIPLHR = Profondità dello spostamento
    NodiIPLHR = -1*cell2mat(NodoInPlaceLinkHR(:,2:3));  
    NodiIPLHR(:,2) = NodiIPLHR(:,2) + Shift;
    
    % Costruisco il segmento di pertinenza
    spe0 = 0; % segmento di pertinenza dell'ancora
    if rIPLHR == 1 % c'è una sola ampolla
       SpeIPLHR = [0; 0.5]; % lo impongo di default
    else
        spe1 = Nodi(1,2)-((NodiIPLHR(1,2) + NodiIPLHR(2,2))/2); % segmento di pertinenza del primo nodo
        if rIPLHR>2
            spe2 = zeros(rIPLHR-2,1);
            for i = 2:(rIPLHR-1)      
                media1 = (abs(NodiIPLHR(i,2)) + abs(NodiIPLHR(i-1,2)))/2;
                media2 = (abs(NodiIPLHR(i,2)) + abs(NodiIPLHR(i+1,2)))/2;
                spei = media1 - media2;
                spe2(i-1,1) = spei;
            end
        else
            spe2 = [];
        end
        spelast = (NodiIPLHR(rIPLHR-1,2)+NodiIPLHR(rIPLHR,2))/2; % segmento di pertinenza del nodo superficiale
        SpeIPLHR = [spe0; spe1; spe2; spelast];    
    end

    % Calcolo la profondità dello spostamento relativo 
    ps0 = (-1)*Nodi(1,2); % profondità dell'ancora
    if rIPLHR == 1 % c'è una sola ampolla
       PsIPLHR = [ps0; -1*NodiIPLHR(1,2)];
    else
        if spe1 <= 1
            ps1 = (-1)*Nodi(1,2)+spe1; % la profondità del primo nodo IPLHR è data
                                   % da ancora + segmento di pertinenza del primo nodo
        else
            ps1 = cell2mat(NodoInPlaceLinkHR(1,3))+0.5+Shift;
        end
        ps2 = zeros(rIPLHR-1,1);
        for j = 2:rIPLHR
            if  j == 2
                if SpeIPLHR(j+1,1) <= 1
                    psj = ps1 + SpeIPLHR(j+1,1);
                else
                    psj = cell2mat(NodoInPlaceLinkHR(j,3))+0.5+Shift;
                end
                % la profondità a cui il nodo j di tipo Tilt Link HR rileva lo 
                % spostamento è data dalla somma fra il suo segmento di 
                % pertinenza e la profondità a cui rileva lo spostamento 
                % il nodo precedente
                psPrec = psj; % mi serve come appoggio per il calcolo della 
                        % profondità dello spostamento del nodo successivo
            else
                if SpeIPLHR(j+1,1) <= 1
                    psj = psPrec + SpeIPLHR(j+1,1);
                else
                    psj = cell2mat(NodoInPlaceLinkHR(j,3))+0.5+Shift;
                end
                psPrec = psj;
            end   
            ps2(j-1,1) = psj;
        end
        PsIPLHR = [ps0; ps1; ps2];
    end
    for i = 2:rIPLHR+1
        if SpeIPLHR(i) > 1
           SpeIPLHR(i) = 1;
        end
    end
    
    if  yesIPL == 1 % Se ci sono In Place Link, il segmento di pertinenza 
        % dell'ampolla lo scrivo considerando i nodi In Place Link sopra e
        % sotto (se sono presenti)
        salta = zeros(rIPLHR,1);
        esco = 0; % entra o meno nel ciclo successivo

        if yesIPLHR3D == 1
           [dim,~] = size(NodoInPlaceLinkHR3D);
           if rIPLHR == dim % vuol dire che tutti i nodi sono dopppi
              esco = 1; % non entro nel ciclo successivo
           end
        end
        
        if  esco == 0
            NodiIPLdown = zeros(rIPLHR,1);
            NodiIPLup = zeros(rIPLHR,1);
            ProfIPLHR = zeros(rIPLHR*3,1);
            for i = 1:rIPLHR
                prof = NodiIPLHR(i,2);
                % cerco il nodo sotto all'Ampolla
                B = NodiIPL(:,2) > prof; % uso > perchè il segno della profondità è +
                A = NodiIPL(B,2);
                if  i == 1 % guardo se il primo nodo è Ampolla o Mems
                    C = isempty(A);
                    if C == 1 % il primo nodo è Ampolla
                        salta(i) = 1;
                    else
                        salta(i) = 0;
                    end
                end
                NodoIPLdown = A(end); % nodo In Place Link sotto all'Ampolla
                
                % cerco il nodo dopo l'Ampolla
                B = NodiIPL(:,2) < prof;
                A = NodiIPL(B,2);
                CC = isempty(A);
                if C == 1 % Manca il nodo Sotto
                    salta(i) = 1;
                else
                    if CC == 1 % ovvero se In Place Link e In Place Link HR sono nello
                          % stesso nodo doppio, ignoro il passaggio (manca
                          % il nodo sopra)
                       salta(i) = 1;
                    else
                        salta(i) = 0;
                        NodoIPLup = A(1);
                        NodiIPLdown(i,1) = NodoIPLdown;
                        NodiIPLup(i,1) = NodoIPLup; % Nodo In Place Link sopra all'ampolla
                    end
                end
            end

            spe0 = 0; % segmento di pertinenza dell'ancora
            if salta(1) == 1
               spe1 = Nodi(1,2)-NodiIPLHR(1,2); % segmento di pertinenza del primo nodo
               ii = 2;
            else
                spe1 = Nodi(1,2)-NodiIPLdown(1,1); % segmento di pertinenza del 
                % primo nodo. N.B.: Per funzionare correttamente, il primo nodo 
                % dopo l'ancora deve per forza essere un Tilt Link!
                ii = 1;
            end
            spe2 = zeros(rIPLHR,1);
            for i = ii:rIPLHR
                spei = (NodiIPLdown(i,1)-NodiIPLup(i,1))/2;
                spe2(i,1) = spei;
            end
            SpeIPLHR = [spe0; spe1; spe2];

            % Calcolo profondità dello spostamento relativo
            m = 1;
            
            for i = 1:rIPLHR
                if salta(i) == 1
                   if i == 1 % considero l'ancora
                        a = (Nodi(i,2)+NodiIPLHR(i,2))/2;
                        ProfIPLHR = PsIPLHR;
                    else
                        a = (NodiIPLHR(i-1,2)+NodiIPLHR(i,2))/2;
                    end 
                    if i == rIPLHR % ultimo nodo
                       if salta(i) == 1    
                          b = NodiIPLHR(i,2);
                          c = NodiIPLHR(i,2)+0.25; 
                       else    
                            b = (NodiIPLHR(i,2)+NodiIPLup(i))/2;
                            c = NodiIPLup(i)-(NodiIPLHR(i,2)-NodiIPLup(i))/2;
                       end
                    else
                       if salta(i+1) == 1
                            b = (NodiIPLHR(i,2)+NodiIPLHR(i+1,2))/2;
                            c = (NodiIPLHR(i+1,2)-(NodiIPLHR(i,2)-NodiIPLHR(i+1,2)))/2;
                        else
                            b = (NodiIPLHR(i,2)+NodiIPLup(i))/2;
                            c = NodiIPLup(i)-(NodiIPLHR(i,2)-NodiIPLup(i))/2;
                       end
                    end
                else
                    a = (NodiIPLdown(i)+NodiIPLHR(i,2))/2;
                    b = (NodiIPLHR(i,2)+NodiIPLup(i))/2;
                    c = (NodiIPLup(i)-(NodiIPLHR(i,2)-NodiIPLup(i))/2);
                end
                ProfIPLHR(m,1) = a;
                ProfIPLHR(m+1,1) = b;
                ProfIPLHR(m+2,1) = c;
                m = m+3;
            end
        end
    end  
   text = 'Segments of relevance and calculation points of In Place Link HR defined correctly';
   fprintf(fileID,fmt,text);
else
    % Non ci sono nodi In Place Link HR
    PsIPLHR = [];
    SpeIPLHR = [];
end

%% In Place Link HR 3D
if  yesIPLHR3D == 1
    NodoIPLHR3D = zeros(rIPL,1);
    t = 1;
    for i=1:rIPL
        for j=1:rIPLHR
            if NodiIPL(i,2)==NodiIPLHR(j,2)
               NodoIPLHR3D(t,1)=NodiIPL(i,2);
               SpeIPLHR(j+1,1) = SpeIPL(i+1,1);
               PsIPLHR(j+1,1) = PsIPL(i+1,1);
               % il segmento di pertinenza dell'ampolla è identico al SP
               % del corrispondente MEMS
               t=t+1;
            end
        end
    end
end

% Chiudo Testo
fclose(fileID);

end