%% Funzione che calcola gli spostamenti in modalità biassiale per i Tilt Link HD privi di magnetometro
function [X_HDVR,Y_HDVR,Z_HDVR,Xlocal_HDVR,Ylocal_HDVR,Zlocal_HDVR,HShift_HDVR,HShift_local_HDVR,...
    AlfaX_HDVR,AlfaY_HDVR,Azimuth_HDVR,Speed_local_HDVR,Speed_HDVR,Acceleration_local_HDVR,...
    Acceleration_HDVR,tempHDVR,ARRAYdateHDVR,ErrTiltLinkHDVR] = biax_HDVR(rHDVR,...
    ACCdef_HDVR,ANGdef_HDVR,ACCdefRisHDVR,tempHDVR,SpeHDVR,PsHDVR,NodoTiltLinkHDVR,tolleranzaAcc,...
    DatiElabTiltLinkHDVR,Ndevst,Wdevst,ARRAYdateHDVR,Tmax,Tmin,NuovoZeroHDVR,...
    NdatiMedia,Ndatidespike,ErrTiltLinkHDVR,margine,Corr_Azimuth,datainiHDVR,IDcentralina,DTcatena,FileName)
 
%% Inizializzazione
fileID = fopen(FileName,'a');
fmt = '%s \r';
text = 'biax_HD function started';
fprintf(fileID,fmt,text);

if NuovoZeroHDVR == 1
    if NdatiMedia > Ndatidespike
        Ndati = NdatiMedia;
    else
        Ndati = Ndatidespike;
    end
    ini = round(Ndati/2)+1;
    if rem(Ndati,2) == 0
        ini = ini+1;
    end
    clear NDati
    ini = ini + margine;
    if ini < 6
        ini = 6;
    end
    [letture,~]=size(DatiElabTiltLinkHDVR);
    if ini > letture
        ini = letture-1;
    end
    if Ndevst ~= 0 % Allora prendo tutti i dati e solo in seguito considero i nuovi, a valle della funzione filtro
        ini = 1;
    end
    ACCdefRisHDVR = ACCdefRisHDVR(ini:end,:);
    ACCdef_HDVR = ACCdef_HDVR(ini:end,:);
    tempHDVR = tempHDVR(ini:end,:);
    DatiElabTiltLinkHDVR = DatiElabTiltLinkHDVR(ini:end,:);
    ARRAYdateHDVR = ARRAYdateHDVR(ini:end,1); 
    ErrTiltLinkHDVR = ErrTiltLinkHDVR(ini:end,:);
end

%% Definisco i dati
Nnodi = rHDVR;
[r,~] = size(ACCdef_HDVR); % Numero di dati
[Ndati,~] = size(ARRAYdateHDVR);
ax = zeros(r,Nnodi);
ay = zeros(r,Nnodi);
az = zeros(r,Nnodi);
for i=1:Nnodi
    ax(:,i) = ACCdef_HDVR(:,(i-1)*3+1); % ax
    ay(:,i) = ACCdef_HDVR(:,(i-1)*3+2:(i-1)*3+2); % ay
    az(:,i) = ACCdef_HDVR(:,(i-1)*3+3:(i-1)*3+3); % az
    angx(:,i) = ANGdef_HDVR(:,(i-1)*3+1);
    angy(:,i) = ANGdef_HDVR(:,(i-1)*3+2:(i-1)*3+2);
end

%% Costruzione delle matrici spostamento e rotazione
GuidaA = zeros(Nnodi,Ndati); % in riga i nodi, in colonna le date
GuidaB = zeros(Nnodi,Ndati); % in riga i nodi, in colonna le date
Zlocal_HDVR = zeros(Nnodi,Ndati);
 
% parametri per il calcolo
SpeHDVR = SpeHDVR(2:end,1); % salto il segmento di pertinenza dell'ancora

% Inizio del ciclo di elaborazione
text = 'Biaxial Elaboration of Tilt Link HD VR started';
fprintf(fileID,fmt,text);

% Inizializzo le matrici
xAcc = ax';
yAcc = ay';
xAng = angx';
yAng = angy';
for d = 1:Ndati
    for n = 1:rHDVR
        % calcolo spostamenti lungo x e y
        GuidaA(n,d) = SpeHDVR(n)*xAcc(n,d);
        GuidaB(n,d) = SpeHDVR(n)*yAcc(n,d);
        % Calcolo l'abbassamento
        sZx(n,d) = SpeHDVR(n)*cos(deg2rad(xAng(n,d)));
        Zx = SpeHDVR(n) - sZx(n,d);
        sZy(n,d) = SpeHDVR(n)*cos(deg2rad(yAng(n,d)));
        Zy = SpeHDVR(n) - sZy(n,d);
        Zlocal_HDVR(n,d) = max(Zx,Zy);
    end
end

text = 'Biaxial calculation executed';
fprintf(fileID,fmt,text);

% dNS e dEO raccolgono i dati del singolo nodo nella singola data
dA = diff(GuidaA,1,2);
dB = diff(GuidaB,1,2);
dZ = diff(Zlocal_HDVR,1,2);
    
%% Controllo delle risultanti di accelerazione
clear rr
clear c
clear cc
ACCdefRisHDVR = ACCdefRisHDVR';  % Nodi in riga, date in colonna
[r,c] = size(ACCdefRisHDVR);
[rr,cc] = size(GuidaA);

% controllo che le matrici con le risultanti delle accelerazioni e 
% le matrici con i dati di spostamento abbiano le stesse dimensioni
if r~=rr
    text = '---Warning! Number of row of displacement data do not correspond to the number of acceleration cosine vector!---';
    fprintf(fileID,fmt,text);
end
if c~=cc
    text = '---Warning! Number of column of displacement data do not correspond to the number of acceleration cosine vector!---';
    fprintf(fileID,fmt,text);
end
clear i
clear j
cont = 1; % contatore
cont2 = 1; % contatore
cont3 = 1; % contatore
tempHDVR = tempHDVR';
textA = 'There are not correction of Tilt Link HD VR based on acceleration vectors filter';
textA2 = 'There are not correction of Tilt Link HD VR based on uncalibrated acceleration vectors';
textT = 'There are not correction of Tilt Link HD VR based on temperature filter';
for j = 2:c % Data
    for i = 1:r % Nodo
        % se il valore assoluto della differenza è maggiore della
        % tolleranza, pongo gli spostamenti giornalieri pari a 0
        if abs(ACCdefRisHDVR(i,j)-ACCdefRisHDVR(i,j-1)) > tolleranzaAcc
            dA(i,j-1) = 0;
            dB(i,j-1) = 0;
            dZ(i,j-1) = 0;  
            textA = ['' num2str(cont) ' correction executed for Tilt Link HD VR - Acceleration vector filter!'];
            cont = cont+1;
        end
        if ACCdefRisHDVR(i,j) < 0.9 || ACCdefRisHDVR(i,j) > 1.1 % Il nodo è fuori taratura! 
            dA(i,j-1) = 0;
            dB(i,j-1) = 0;
            dZ(i,j-1) = 0;
            tempHDVR(i,j) = tempHDVR(i,j-1);
            textA2 = ['' num2str(cont) ' correction executed for Tilt Link HD VR - uncalibrated Acceleration vector!'];
            cont3 = cont3+1;
        end
    end
end

FileTemperature = ['' IDcentralina '-' DTcatena '-HDVR-Therm.csv'];
if isfile(FileTemperature) == 1
    DatiRaw = csvread(FileTemperature);
    [rDR,cDR] = size(DatiRaw);
    DatiRaw(:,1) = DatiRaw(:,1) + 730000;
else
    rDR = 1;
    cDR = 1;
end
for b = 1:c % Data
    for a = 1:r % Nodo
        % NON considero i dati al di sopra dei 80 °C o al di sotto dei -30 °C! 
        if tempHDVR(a,b) > Tmax || tempHDVR(a,b) < Tmin
            cont2 = cont2+1;
            if b == 1
                if isfile(FileTemperature) == 1
                    RawDate = find(DatiRaw(:,1)<=datenum(datainiHDVR));
                    if isempty(RawDate) == 1
                        cc = 2;
                        while cc <= c
                            if tempHDVR(a,cc) > Tmax || tempHDVR(a,cc) < Tmin
                                cc = cc+1;
                            else
                                break
                            end
                        end
                        tempHDVR(a,b) = tempHDVR(a,cc);
                    else
                        if isnan(DatiRaw(RawDate(end),a+1)) == 0
                            tempHDVR(a,b) = DatiRaw(RawDate(end),a+1);
                            ErrTiltLinkHDVR(b,a) = 0.5;
                            wardat = 'Temperature data of Tilt Link HD VR nodes corrected using Raw Data of reference Csv file.';
                            fprintf(fileID,fmt,wardat);
                        else
                            cc = 2;
                            while cc <= c
                                if tempHDVR(a,cc) > Tmax || tempHDVR(a,cc) < Tmin
                                    cc = cc+1;
                                else
                                    break
                                end
                            end
                            tempHDVR(a,b) = tempHDVR(a,cc);
                        end
                    end
                else
                    cc = 2;
                    while cc <= c
                        if tempHDVR(a,cc) > Tmax || tempHDVR(a,cc) < Tmin
                            cc = cc+1;
                        else
                            break
                        end
                    end
                    tempHDVR(a,b) = tempHDVR(a,cc);
                end
            else
                tempHDVR(a,b) = tempHDVR(a,b-1);
                dA(a,b-1) = 0;
                dB(a,b-1) = 0;
                dZ(a,b-1) = 0;
                ErrTiltLinkHDVR(b,a) = 0.5;
            end
            textT = ['' num2str(cont2) ' correction executed for Tilt Link HD VR - Temperature filter!'];
        end
    end
end

if  rDR~=1 && cDR~=1 && isempty(DatiRaw) == 0
    RawDate1 = find(DatiRaw(:,1)<=ARRAYdateHDVR(1));
    if isempty(RawDate1) == 1
        RawDate2 = 1;
    elseif RawDate1(end) == rDR
        RawDate2 = find(ARRAYdateHDVR(:,1)>DatiRaw(end,1));
    else
        RawDate2 = find(ARRAYdateHDVR(:,1)>DatiRaw(RawDate1(end)+1,1));
    end
else
    RawDate1 = [];
    RawDate2 = 1;
end
if isempty(RawDate1) == 0 && isempty(RawDate2) == 0
    Dati = [DatiRaw(1:RawDate1(end),:); ARRAYdateHDVR(RawDate2(1):end) tempHDVR(:,RawDate2(1):end)'];
elseif isempty(RawDate1) == 1 && isempty(RawDate2) == 0
    Dati = [ARRAYdateHDVR tempHDVR'];
else
    Dati = DatiRaw;
end
% Elimino appoggio più vecchio di un mese
RawDate3 = find(Dati(:,1)<now-30);
if isempty(RawDate3) == 0
    [rDati,~] = size(Dati);
    if RawDate3(end) == rDati
    else
        Dati = Dati(RawDate3(end)+1:end,:);
    end
end
if isfile(FileTemperature) == 1
    delete(FileTemperature);
end
Dati(:,1) = Dati(:,1) - 730000;
csvwrite(FileTemperature,Dati);

tempHDVR = tempHDVR';

fprintf(fileID,fmt,textA);
fprintf(fileID,fmt,textA2);
fprintf(fileID,fmt,textT);
fclose(fileID);
            
% azzeramenti di alcuni nodi in particolare
[dA,dB,dZ] = azzeramenti(IDcentralina,DTcatena,dA,dB,dZ,NodoTiltLinkHDVR,FileName);

% Filtro
[dA,dB,dZ] = filtro(dA,dB,dZ,Ndevst,Wdevst,FileName);
if NuovoZeroHDVR == 1 && Ndevst ~= 0
    if NdatiMedia > Ndatidespike
        NdatiF = NdatiMedia;
    else
        NdatiF = Ndatidespike;
    end
    ini = round(NdatiF/2)+1;
    if rem(NdatiF,2) == 0
        ini = ini+1;
    end
    clear NDatiF
    iniST = round(Wdevst/2);
    if rem(Wdevst,2) == 0
        iniST = iniST+1;
    end
    iniST = iniST + margine;
    if iniST > ini
        ini = iniST;
    end
    if ini < 6
        ini = 6;
    end
    dA = dA(:,ini:end);
    dB = dB(:,ini:end);
    dZ = dZ(:,ini:end);
    tempHDVR = tempHDVR(ini:end,:);
    DatiElabTiltLinkHDVR = DatiElabTiltLinkHDVR(ini:end,:);
    ARRAYdateHDVR = ARRAYdateHDVR(ini:end,1); 
    ErrTiltLinkHDVR = ErrTiltLinkHDVR(ini:end,:);
end
    
%% Finalizzo i calcoli
[rNS,cNS] = size(dA);
sommaX = zeros(rHDVR,1);
Xlocal_HDVR = zeros(rNS,cNS+1); % locale nello spazio, cumulato nel tempo
AlfaX_HDVR = zeros(rNS,cNS+1); % Angoli
sommaY = zeros(rHDVR,1);
Ylocal_HDVR = zeros(rNS,cNS+1);
AlfaY_HDVR = zeros(rNS,cNS+1); % Angoli
sommaZ = zeros(rNS,cNS);
Zlocal_HDVR = zeros(rNS,cNS+1);
X_HDVR = zeros(rNS,cNS+1); % cumulato nel tempo e nello spazio
Y_HDVR = zeros(rNS,cNS+1); 
Z_HDVR = zeros(rNS,cNS+1);
HShift_HDVR = zeros(rNS,cNS+1); % massima pendenza cumulato nel tempo e nello spazio
HShift_local_HDVR = zeros(rNS,cNS+1); % massima pendenza locale
Azimuth_HDVR = zeros(rNS,cNS+1); % azimut
azim = zeros(rNS,cNS+1); % matrice di appoggio per il calcolo dell'azimuth
Speed_HDVR = zeros(rHDVR,cNS+1); % Velocità 2D Cumulata
Speed_local_HDVR = zeros(rHDVR,cNS+1); % Velocità 2D locale
Acceleration_HDVR = zeros(rHDVR,cNS+1); % Accelerazione 2D Cumulata
Acceleration_local_HDVR = zeros(rHDVR,cNS+1); % Accelerazione 2D Locale

% Recupero i dati già elaborati
if NuovoZeroHDVR == 1
   [rE,cE] = size(DatiElabTiltLinkHDVR);
   cont = 3;
   n = 1;
   while cont<=cE
         sommaX(n,1) = cell2mat(DatiElabTiltLinkHDVR(1,cont))';
         Xlocal_HDVR(n,1) = sommaX(n,1);
         X_HDVR(n,1) = cell2mat(DatiElabTiltLinkHDVR(1,cont+3))';
         sommaY(n,1) = cell2mat(DatiElabTiltLinkHDVR(1,cont+1))';
         Ylocal_HDVR(n,1) = sommaY(n,1);
         Y_HDVR(n,1) = cell2mat(DatiElabTiltLinkHDVR(1,cont+4))';
         for j = 1:rHDVR
             AlfaX_HDVR(j,1) = asind(Xlocal_HDVR(j,1)/SpeHDVR(j));
             AlfaY_HDVR(j,1) = asind(Ylocal_HDVR(j,1)/SpeHDVR(j));
         end
         Zlocal_HDVR(n,1) = cell2mat(DatiElabTiltLinkHDVR(1,cont+2))';
         Z_HDVR(n,1) = cell2mat(DatiElabTiltLinkHDVR(1,cont+5))';
         HShift_HDVR(n,1) = cell2mat(DatiElabTiltLinkHDVR(1,cont+6))';
         HShift_local_HDVR(n,1) = cell2mat(DatiElabTiltLinkHDVR(1,cont+7))';
         Azimuth_HDVR(n,1) = cell2mat(DatiElabTiltLinkHDVR(1,cont+8))';
         Speed_HDVR(n,1:rE) = cell2mat(DatiElabTiltLinkHDVR(:,cont+10))';
         Speed_local_HDVR(n,1:rE) = cell2mat(DatiElabTiltLinkHDVR(:,cont+11))';
         Acceleration_HDVR(n,1:rE) = cell2mat(DatiElabTiltLinkHDVR(:,cont+12))';
         Acceleration_local_HDVR(n,1:rE) = cell2mat(DatiElabTiltLinkHDVR(:,cont+13))';
         cont=cont+16;
         n = n+1;
   end
else
    Zlocal_HDVR(:,1) = SpeHDVR;
    Z_HDVR(:,1) = PsHDVR(2:end);
end

% elaboro i dati nuovi
for iii = 1:cNS
    Xlocal_HDVR(:,iii+1) = sum(dA(:,1:iii),2)+sommaX(:,1);
    Ylocal_HDVR(:,iii+1) = sum(dB(:,1:iii),2)+sommaY(:,1);
    for j = 1:rHDVR
        AlfaX_HDVR(j,iii+1) = asind(Xlocal_HDVR(j,iii+1)/SpeHDVR(j));
        AlfaY_HDVR(j,iii+1) = asind(Ylocal_HDVR(j,iii+1)/SpeHDVR(j));
    end
    sommaZ(:,iii+1) = sum(dZ(:,1:iii),2);
    X_HDVR(:,iii+1) = cumsum(Xlocal_HDVR(:,iii+1));
    Y_HDVR(:,iii+1) = cumsum(Ylocal_HDVR(:,iii+1));
    Z_HDVR(:,iii+1) = cumsum(sommaZ(:,iii+1))+ Z_HDVR(:,1);
    HShift_HDVR(:,iii+1) = (X_HDVR(:,iii+1).^2+Y_HDVR(:,iii+1).^2).^0.5;
    HShift_local_HDVR(:,iii+1) = (Xlocal_HDVR(:,iii+1).^2+Ylocal_HDVR(:,iii+1).^2).^0.5;
    Zlocal_HDVR(:,iii+1) = sommaZ(:,iii+1) + SpeHDVR; % Zeta è il singolo abbassamento di quel nodo
    for rr = 1:rHDVR
        azim(rr,iii) = (acos(abs(X_HDVR(rr,iii))/HShift_HDVR(rr,iii)))*180/3.141592654; % Angolo Teta in gradi 0° - 90°
        segnoNS = sign(X_HDVR(rr,iii));
        segnoEO = sign(Y_HDVR(rr,iii));
        % L'azimuth si calcola con NS = 0, positivo in senso orario
        % (90° = Est)
        if segnoNS == 1 && segnoEO == 1 % quadrante 1
            Azimuth_HDVR(rr,iii+1) = azim(rr,iii); % Teta lo tengo come è (1 quadrante)
        elseif segnoNS == -1 && segnoEO == 1 % quadrante 2
               Azimuth_HDVR(rr,iii+1) = 180 - azim(rr,iii); % 180-teta
        elseif segnoNS == -1 && segnoEO == -1 % quadrante 3
               Azimuth_HDVR(rr,iii+1) = 180 + azim(rr,iii); % 180+teta
        elseif segnoNS == 1 && segnoEO == -1 % quadrante 4
               Azimuth_HDVR(rr,iii+1) = 360 - azim(rr,iii); % 360-teta
        elseif segnoNS == 0 && segnoEO == -1 % 270°
               Azimuth_HDVR(rr,iii+1) = 270;
        elseif segnoNS == -1 && segnoEO == 0 % 180°
               Azimuth_HDVR(rr,iii+1) = 180;
        end
    end
end
[rAz,cAz] = size(Azimuth_HDVR);
for n=1:rAz
    for c=2:cAz
        Azimuth_HDVR(n,c) = real(Azimuth_HDVR(n,c))+Corr_Azimuth;
        if Azimuth_HDVR(n,c)>=360
            Azimuth_HDVR(n,c)=Azimuth_HDVR(n,c)-360;
        end
    end
end

%% Calcolo velocità di spostamento giornaliera
[numDate,~] = size(ARRAYdateHDVR); % numero di date
d = 1;
p = 1;
diffDate = 0;
n = 1;
period = 1; % calcolo giornaliero
ContSUP = [];
for dd = 1:numDate
    while diffDate < period
          d = d+1;  
          if d > numDate % Se d supera le date disponibili, esco dal ciclo while
              break
          end
          diffDate = ARRAYdateHDVR(d) - ARRAYdateHDVR(p);
    end
    if d >numDate
        break
    end
    ContSUP(n,1) = d; %#ok<*AGROW> % Creo matrice indici dell'estremo superiore della differenza
    ContINF(n,1) = p; % Creo matrice indici dell'estremo inferiore della differenza
    p = p+1; % passo alla data di partenza successiva 
    d = p; % resetto il conto di d
    n = n+1;
    diffDate = 0;
end

check = isempty(ContSUP);
if check == 0
    [nDate,~] = size(ContSUP);
    for s = 1:rHDVR
        N = 1;
        for dd = 1:nDate
            Speed_HDVR(s,ContSUP(N,1)) = (HShift_HDVR(s,ContSUP(N,1))-HShift_HDVR(s,ContINF(N,1)))/(ARRAYdateHDVR(ContSUP(N,1))-ARRAYdateHDVR(ContINF(N,1)));
            Speed_local_HDVR(s,ContSUP(N,1)) = (HShift_local_HDVR(s,ContSUP(N,1))-HShift_local_HDVR(s,ContINF(N,1)))/(ARRAYdateHDVR(ContSUP(N,1))-ARRAYdateHDVR(ContINF(N,1)));
            Acceleration_HDVR(s,ContSUP(N,1)) = (Speed_HDVR(s,ContSUP(N,1))-Speed_HDVR(s,ContINF(N,1)))/(ARRAYdateHDVR(ContSUP(N,1))-ARRAYdateHDVR(ContINF(N,1)));
            Acceleration_local_HDVR(s,ContSUP(N,1)) = (Speed_local_HDVR(s,ContSUP(N,1))-Speed_local_HDVR(s,ContINF(N,1)))/(ARRAYdateHDVR(ContSUP(N,1))-ARRAYdateHDVR(ContINF(N,1)));
            N = N+1;
        end
    end
end

% Approssimo i dati con il corretto numero di cifre decimali
[X_HDVR,Y_HDVR,Z_HDVR,Xlocal_HDVR,Ylocal_HDVR,Zlocal_HDVR,HShift_HDVR,HShift_local_HDVR,Azimuth_HDVR,Speed_HDVR,...
    Speed_local_HDVR,Acceleration_HDVR,Acceleration_local_HDVR,tempHDVR] = approx_HD(X_HDVR,Y_HDVR,Z_HDVR,...
    Xlocal_HDVR,Ylocal_HDVR,Zlocal_HDVR,HShift_HDVR,HShift_local_HDVR,Azimuth_HDVR,Speed_HDVR,Speed_local_HDVR,...
    Acceleration_HDVR,Acceleration_local_HDVR,tempHDVR,FileName);

% Riordino matrice errori
[r,~] = size(ErrTiltLinkHDVR);
Matrice_err = zeros(r,rHDVR);
if cell2mat(NodoTiltLinkHDVR(1,5)) == 1 % è presente il magnetometro
    col = 7;
else
    col = 4;
end
for i = 1:r % date
    d = 1;
    for n = 1:rHDVR % nodi
        j = 1;
        err = ErrTiltLinkHDVR(i,d:d+col-1);
        while j <= col
            if err(1,j) == 1
                Matrice_err(i,n) = 1;
                break
            end
            if err(1,j) == 0.5
                Matrice_err(i,n) = 0.5;
            end
            j = j+1;
        end
        d = d+col;
    end
end
ErrTiltLinkHDVR = Matrice_err';
    
text = 'Tilt Link HD VR biaxial calculation executed correctly. biax_HDVR function ended';
fileID = fopen(FileName,'a');
fmt = '%s \r';
fprintf(fileID,fmt,text);
fclose(fileID);

end