%% Funzione che calcola gli spostamenti in modalità biassiale per i 
% Tilt Link HR H (ampolle)
% Gli output sono: 
% X_TLHRH, Y_TLHRH, Z_TLHRH - dati cumulati di ogni singolo nodo per ogni singola data, 
%   nelle tre direzioni, riferiti al primo dato
% Xlocal_TLHRH, Ylocal_TLHRH, Zlocal_TLHRH - dati cumulati di ogni singolo nodo per ogni singola data, 
%   nelle tre direzioni, riferiti al primo dato
% HShift_local_TLHRH rappresenta lo stesso dato di Z_TLHRH
% I week shift sono i maggiori spostamenti settimanali

function [Y_TLHRH,Z_TLHRH,Ylocal_TLHRH,Zlocal_TLHRH,AlfaX_TLHRH,AlfaY_TLHRH,...
    TempDef_TLHRH,Speed_TLHRH,Speed_local_TLHRH,Acceleration_TLHRH,...
    Acceleration_local_TLHRH,ARRAYdateTLHRH,ErrTiltLinkHRH] = biax_TLHRH(...
    IDcentralina,DTcatena,NodoTiltLinkHRH,rTLHRH,ANGdef_TLHRH,TempDef_TLHRH,SpeTLHRH,PsTLHRH,...
    DatiElabTiltLinkHRH,Ndevst_HR,Wdevst_HR,ARRAYdateTLHRH,NuovoZeroTLHRH,NdatiMedia,...
    Ndatidespike,ErrTiltLinkHRH,Tmax,Tmin,datainiTLHRH,margine,FileName)

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

if NuovoZeroTLHRH == 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
    if Ndevst_HR ~= 0 % Allora prendo tutti i dati e solo in seguito considero i nuovi, a valle della funzione filtro
        ini = 1;
    end
    ANGdef_TLHRH = ANGdef_TLHRH(ini:end,:);
    TempDef_TLHRH = TempDef_TLHRH(ini:end,:);
    DatiElabTiltLinkHRH = DatiElabTiltLinkHRH(ini:end,:);
    ARRAYdateTLHRH = ARRAYdateTLHRH(ini:end,1); 
    ErrTiltLinkHRH = ErrTiltLinkHRH(ini:end,:);
end

%% Definisco i dati
Nnodi = rTLHRH;

% Spacchetto
[r,~] = size(ANGdef_TLHRH); % Numero di dati
[Ndati,~] = size(ARRAYdateTLHRH);
ax = zeros(r,Nnodi);
ay = zeros(r,Nnodi);
for i=1:Nnodi
    ax(:,i) = ANGdef_TLHRH(:,(i-1)*2+1); % ax
    ay(:,i) = ANGdef_TLHRH(:,(i-1)*2+2); % ay
end
ax = ax'; % riga nodi, colonna date
ay = ay'; % riga nodi, colonna date

% Dati locali
asseY_TLHRH = zeros(Nnodi,Ndati); % Raccoglie i dati planari locali nella direzione di sviluppo della catena
asseZ_TLHRH = zeros(Nnodi,Ndati); % Raccoglie i dati planari locali nella direzione di sviluppo della catena, basso->alto

% Dati cumulati
Y_TLHRH = zeros(Nnodi,Ndati); % Raccoglie i dati cumulati nella direzione di sviluppo della catena
Z_TLHRH = zeros(Nnodi,Ndati); % Raccoglie i dati cumulati nella direzione di sviluppo della catena, basso->alto

% Angoli
AlfaX_TLHRH = zeros(Nnodi,Ndati); 
AlfaY_TLHRH = zeros(Nnodi,Ndati);

% parametri per il calcolo
SpeTLHRH = SpeTLHRH(2:end,1); % valori segmenti di pertinenza
PsTLHRH = PsTLHRH(2:end,1); % valori posizioni punti di calcolo

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

% Nodi nuovi (lo devo sapere per le convenzioni)
% I due assi sono a 90° di cui asse Y a 90° verso sinistra rispetto allo
% sviluppo della catena, asse X a 0° coincidente con lo sviluppo della catena.
% L'asse Z è verso il basso.
ax = 0.01745329251994329576923690768489.*ax;
ay = 0.01745329251994329576923690768489.*ay;
for jj = 1:Ndati
    for ii = 1:Nnodi
        % converto gli angoli in radianti (matlab ragiona così)
        [Y_TLHRH,Z_TLHRH,asseY_TLHRH,asseZ_TLHRH,AlfaX_TLHRH,AlfaY_TLHRH]...
            = CalcoloBiax_TLHRH(SpeTLHRH,PsTLHRH,ii,jj,ax,ay,asseY_TLHRH,asseZ_TLHRH,...
            Y_TLHRH,Z_TLHRH,AlfaX_TLHRH,AlfaY_TLHRH);
    end
end
dsY = diff(asseY_TLHRH,1,2);
dsZ = diff(asseZ_TLHRH,1,2);

%% Controllo della temperatura
ANGdef_TLHRH = ANGdef_TLHRH';
[r,c] = size(ANGdef_TLHRH);
r = r/2;
clear i
clear j
contT = 1; % contatore
TempDef_TLHRH = TempDef_TLHRH';
textT = 'There are not correction of Tilt Link HR H based on temperature filter';

FileTemperature = ['' IDcentralina '-' DTcatena '-TLHRH-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 j = 1:c % Data
    for i = 1:r % Nodo
        % NON considero i dati al di sopra di Tmax o al di sotto di Tmin
        if TempDef_TLHRH(i,j) > Tmax || TempDef_TLHRH(i,j) < Tmin
            cont2 = cont2+1;
            if j == 1
                if isfile(FileTemperature) == 1
                    RawDate = find(DatiRaw(:,1)<=datenum(datainiTLHRH));
                    if isempty(RawDate) == 1
                        cc = 2;
                        while cc <= Ndate
                            if TempDef_TLHRH(i,cc) > Tmax || TempDef_TLHRH(i,cc) < Tmin
                                cc = cc+1;
                            else
                                break
                            end
                        end
                        TempDef_TLHRH(i,j) = TempDef_TLHRH(i,cc);
                    else
                        if isnan(DatiRaw(RawDate(end),i+1)) == 0
                            TempDef_TLHRH(i,j) = DatiRaw(RawDate(end),i+1);
                            ErrTiltLinkHRH(j,i) = 0.5;
                            wardat = 'Temperature data of Tilt Link HR H nodes corrected using Raw Data of reference Csv file.';
                            fprintf(fileID,fmt,wardat);
                        else
                            cc = 2;
                            while cc <= c
                                if TempDef_TLHRH(i,cc) > Tmax || TempDef_TLHRH(i,cc) < Tmin
                                    cc = cc+1;
                                else
                                    break
                                end
                            end
                            TempDef_TLHRH(i,j) = TempDef_TLHRH(i,cc);
                        end
                    end
                else
                    cc = 2;
                    while cc <= c
                        if TempDef_TLHRH(i,cc) > Tmax || TempDef_TLHRH(i,cc) < Tmin
                            cc = cc+1;
                        else
                            break
                        end
                    end
                    TempDef_TLHRH(i,j) = TempDef_TLHRH(i,cc);
                end
            else       
                dsY(i,j-1) = 0;
                dsZ(i,j-1) = 0;
                TempDef_TLHRH(i,j) = TempDef_TLHRH(i,j-1);
                ErrTiltLinkHRH(j,i) = 0.5;
            end
        end
        textT = ['' num2str(contT) ' correction executed for Tilt Link HR H - Temperature filter!'];
    end
end

if  rDR~=1 && cDR~=1 && isempty(DatiRaw) == 0
    RawDate1 = find(DatiRaw(:,1)<=ARRAYdateTLHRH(1));
    if isempty(RawDate1) == 1
        RawDate2 = 1;
    elseif RawDate1(end) == rDR
        RawDate2 = find(ARRAYdateTLHRH(:,1)>DatiRaw(end,1));
    else
        RawDate2 = find(ARRAYdateTLHRH(:,1)>DatiRaw(RawDate1(end)+1,1));
    end
else
    RawDate1 = [];
    RawDate2 = 1;
end
if isempty(RawDate1) == 0 && isempty(RawDate2) == 0
    Dati = [DatiRaw(1:RawDate1(end),:); ARRAYdateTLHRH(RawDate2(1):end) TempDef_TLHRH(:,RawDate2(1):end)'];
elseif isempty(RawDate1) == 1 && isempty(RawDate2) == 0
    Dati = [ARRAYdateTLHRH TempDef_TLHRH'];
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);

TempDef_TLHRH = TempDef_TLHRH';
fprintf(fileID,fmt,textT);

% azzeramenti di alcuni nodi in particolare
[dsY,dsZ] = azzeramenti_TLHRH(IDcentralina,DTcatena,dsY,...
    dsZ,NodoTiltLinkHRH,FileName);

%% Filtro
dsX = dsY; % La variabile non è utilizzata
[~,dsY,dsZ] = filtro_TLH(dsX,dsY,dsZ,Ndevst_HR,Wdevst_HR,FileName);
fileID = fopen(FileName,'a');
fprintf(fileID,fmt,text);
if NuovoZeroTLHRH == 1 && Ndevst_HR ~= 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_HR/2);
    if rem(Wdevst_HR,2) == 0
        iniST = iniST+1;
    end
    iniST = iniST + margine;
    if iniST > ini
        ini = iniST;
    end
    if ini < 6
        ini = 6;
    end
    dsY = dsY(:,ini:end);
    dsZ = dsZ(:,ini:end);
    TempDef_TLHRH = TempDef_TLHRH(ini:end,:);
    DatiElabTiltLinkHRH = DatiElabTiltLinkHRH(ini:end,:);
    ARRAYdateTLHRH = ARRAYdateTLHRH(ini:end,1); 
    ErrTiltLinkHRH = ErrTiltLinkHRH(ini:end,:);
end

%% Finalizzo i calcoli
[rx,cx] = size(dsZ);
% totale del singolo nodo data per data
sommaY_TLHRH = zeros(rx,cx+1);
sommaZ_TLHRH = zeros(rx,cx+1);
% cumulata del singolo nodo data per data in posizione assoluta
% il primo valore è quello di posizione assoluta iniziale
Y_TLHRH = zeros(rx,cx+1);
Z_TLHRH = zeros(rx,cx+1);
% cumulata del singolo nodo data per data in posizione relativa
% il primo valore è quello di posizione assoluta iniziale
Ylocal_TLHRH = zeros(Nnodi,Ndati);
Zlocal_TLHRH = zeros(Nnodi,Ndati);
Speed_TLHRH = zeros(Nnodi,Ndati);
Speed_local_TLHRH = zeros(Nnodi,Ndati);
Acceleration_TLHRH = zeros(Nnodi,Ndati);
Acceleration_local_TLHRH = zeros(Nnodi,Ndati);

if NuovoZeroTLHRH == 1
    [rE,cE] = size(DatiElabTiltLinkHRH);
    cont = 4;
    n = 1;
    while cont<=cE
        sommaY_TLHRH(n,1) = cell2mat(DatiElabTiltLinkHRH(1,cont))';
        Ylocal_TLHRH(n,1) = sommaY_TLHRH(n,1);
        sommaZ_TLHRH(n,1) = cell2mat(DatiElabTiltLinkHRH(1,cont+1))';
        Zlocal_TLHRH(n,1) = sommaZ_TLHRH(n,1);
        Y_TLHRH(n,1) = cell2mat(DatiElabTiltLinkHRH(1,cont+3))';
        Z_TLHRH(n,1) = cell2mat(DatiElabTiltLinkHRH(1,cont+4))';
        Speed_local_TLHRH(n,1:rE) = cell2mat(DatiElabTiltLinkHRH(:,cont+7))';
        Speed_TLHRH(n,1:rE) = cell2mat(DatiElabTiltLinkHRH(:,cont+6))';
        Acceleration_local_TLHRH(n,1:rE) = cell2mat(DatiElabTiltLinkHRH(:,cont+9))';
        Acceleration_TLHRH(n,1:rE) = cell2mat(DatiElabTiltLinkHRH(:,cont+8))';
        cont = cont+13;
        n = n+1;
    end
else
    % Primo dato
    Y_TLHRH(:,1) = PsTLHRH;
    Z_TLHRH(:,1) = 0;
end
for iii = 1:cx % date
    % somma singoli spostamenti del singolo nodo
    sommaY_TLHRH(:,iii+1) = sum(dsY(:,1:iii),2)+sommaY_TLHRH(:,1);
    sommaZ_TLHRH(:,iii+1) = sum(dsZ(:,1:iii),2)+sommaZ_TLHRH(:,1);
    % spostamenti locali riferiti allo 0
    Ylocal_TLHRH(:,iii+1) = sommaY_TLHRH(:,iii+1);
    Zlocal_TLHRH(:,iii+1) = sommaZ_TLHRH(:,iii+1);
    % cumulata spostamenti
    Y_TLHRH(:,iii+1) = Y_TLHRH(:,1) + cumsum(sommaY_TLHRH(:,iii+1)) - cumsum(sommaY_TLHRH(:,1));
    Z_TLHRH(:,iii+1) = Z_TLHRH(:,1) + cumsum(sommaZ_TLHRH(:,iii+1)) - cumsum(sommaZ_TLHRH(:,1));
end

%% Calcolo velocità di spostamento giornaliera
[numDate,~] = size(ARRAYdateTLHRH); % 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 = ARRAYdateTLHRH(d) - ARRAYdateTLHRH(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:rTLHRH
        N = 1;
        for dd = 1:nDate
            Speed_TLHRH(s,ContSUP(N,1)) = (Z_TLHRH(s,ContSUP(N,1))-Z_TLHRH(s,ContINF(N,1)))/...
                (ARRAYdateTLHRH(ContSUP(N,1))-ARRAYdateTLHRH(ContINF(N,1)));
            Speed_local_TLHRH(s,ContSUP(N,1)) = (Zlocal_TLHRH(s,ContSUP(N,1))-Zlocal_TLHRH(s,ContINF(N,1)))/...
                (ARRAYdateTLHRH(ContSUP(N,1))-ARRAYdateTLHRH(ContINF(N,1)));
            Acceleration_TLHRH(s,ContSUP(N,1)) = (Speed_TLHRH(s,ContSUP(N,1))-Speed_TLHRH(s,ContINF(N,1)))/...
                (ARRAYdateTLHRH(ContSUP(N,1))-ARRAYdateTLHRH(ContINF(N,1)));
            Acceleration_local_TLHRH(s,ContSUP(N,1)) = (Speed_local_TLHRH(s,ContSUP(N,1))-Speed_local_TLHRH(s,ContINF(N,1)))/...
                (ARRAYdateTLHRH(ContSUP(N,1))-ARRAYdateTLHRH(ContINF(N,1)));
            N = N+1;
        end
    end
end
    
fclose(fileID);
% Approssimo i dati con il corretto numero di cifre decimali
[Y_TLHRH,Z_TLHRH,Ylocal_TLHRH,Zlocal_TLHRH,Speed_TLHRH,Speed_local_TLHRH,...
    Acceleration_TLHRH,Acceleration_local_TLHRH,TempDef_TLHRH] = approx_TLHRH(...
    Y_TLHRH,Z_TLHRH,Ylocal_TLHRH,Zlocal_TLHRH,Speed_TLHRH,Speed_local_TLHRH,...
    Acceleration_TLHRH,Acceleration_local_TLHRH,TempDef_TLHRH,FileName);

% Riordino matrice errori
[r,~] = size(ErrTiltLinkHRH);
Matrice_err = zeros(r,rTLHRH);
for i = 1:r % date
    d = 1;
    for n = 1:rTLHRH % nodi
        j = 1;
        err = ErrTiltLinkHRH(i,d:d+5);
        while j <= 6
            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+6;
    end
end
ErrTiltLinkHRH = Matrice_err';
    
text = 'Tilt Link HR H calculation executed correctly';
fileID = fopen(FileName,'a');
fmt = '%s \r';
fprintf(fileID,fmt,text);
fclose(fileID);

end