% Questa elaborazione semplificata NON usa i quaternioni e i dati di campo
% magnetico

function [Y_TLH,Z_TLH,Xlocal_TLH,Ylocal_TLH,Zlocal_TLH,AlfaX_TLH,AlfaY_TLH,TempDef_TLH,...
    Speed_TLH,Speed_local_TLH,Acceleration_TLH,Acceleration_local_TLH,ARRAYdateTLH,...
    ErrTiltLinkH] = biax_TLH(IDcentralina,DTcatena,rTLH,ACCdef_TLH,ACCdefRis_TLH,...
    TempDef_TLH,SpeTLH,PsTLH,tolleranzaAcc,DatiElabTiltLinkH,Ndevst,Wdevst,ARRAYdateTLH,...
    NuovoZeroTLH,Tmax,Tmin,NodoTiltLinkH,NdatiMedia,Ndatidespike,Traversine,ErrTiltLinkH,...
    margine,FileName)
        
fileID = fopen(FileName,'a');
fmt = '%s \r';
text = 'biax_TLH function started';
fprintf(fileID,fmt,text);

if NuovoZeroTLH == 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 ~= 0 % Allora prendo tutti i dati e solo in seguito considero i nuovi, a valle della funzione filtro
        ini = 1;
    end
    ACCdef_TLH = ACCdef_TLH(ini:end,:);
    ACCdefRis_TLH = ACCdefRis_TLH(ini:end,:);
    TempDef_TLH = TempDef_TLH(ini:end,:);
    DatiElabTiltLinkH = DatiElabTiltLinkH(ini:end,:);
    ARRAYdateTLH = ARRAYdateTLH(ini:end,1); 
    ErrTiltLinkH = ErrTiltLinkH(ini:end,:);
end
    
%% Definisco i dati
Nnodi = rTLH;

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

% Dati locali
asseX_TLH = zeros(Nnodi,Ndati); % Raccoglie i dati planari locali nella direzione perpendicolare allo sviluppo della catena
asseY_TLH = zeros(Nnodi,Ndati); % Raccoglie i dati planari locali nella direzione di sviluppo della catena
asseZ_TLH = zeros(Nnodi,Ndati); % Raccoglie i dati planari locali nella direzione di sviluppo della catena, basso->alto

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

% Angoli
AlfaX_TLH = zeros(Nnodi,Ndati); 
AlfaY_TLH = zeros(Nnodi,Ndati);

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

% Inizio del ciclo di elaborazione
text = 'Elaboration of Tilt Link 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.
for jj = 1:Ndati
    for ii = 1:Nnodi
        [Y_TLH,Z_TLH,asseX_TLH,asseY_TLH,asseZ_TLH,AlfaX_TLH,AlfaY_TLH] = CalcoloBiax_TLH(...
            SpeTLH,PsTLH,ii,jj,ax,ay,asseX_TLH,asseY_TLH,asseZ_TLH,Y_TLH,Z_TLH,AlfaX_TLH,...
            AlfaY_TLH,Traversine);
    end
end
dX_TLH = diff(asseX_TLH,1,2);
dY_TLH = diff(asseY_TLH,1,2);
dZ_TLH = diff(asseZ_TLH,1,2);

%% Controllo delle risultanti di accelerazione e campo magnetico
clear r
clear rr
clear c
clear cc
ACCdefRis_TLH = ACCdefRis_TLH';  % Nodi in riga, date in colonna
[r,c] = size(ACCdefRis_TLH);
[rr,cc] = size(asseZ_TLH);

% 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
TempDef_TLH = TempDef_TLH';
textA = 'There are not correction of Tilt Link H based on acceleration vectors filter';
textA2 = 'There are not correction of Tilt Link H based on uncalibrated acceleration vectors';
textT = 'There are not correction of Tilt Link H 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(ACCdefRis_TLH(i,j)-ACCdefRis_TLH(i,j-1)) > tolleranzaAcc 
            dX_TLH(i,j-1) = 0;
            dY_TLH(i,j-1) = 0;
            dZ_TLH(i,j-1) = 0;
            textA = ['' num2str(cont) ' correction executed for Tilt Link H - Acceleration vector filter!'];
            cont = cont+1;
        end
        if ACCdefRis_TLH(i,j) < 0.9 || ACCdefRis_TLH(i,j) > 1.1 % Il nodo è fuori taratura! 
            dX_TLH(i,j-1) = 0;
            dY_TLH(i,j-1) = 0;
            dZ_TLH(i,j-1) = 0;
            TempDef_TLH(i,j) = TempDef_TLH(i,j-1);
            textA2 = ['' num2str(cont3) ' correction executed for Tilt Link H - uncalibrated Acceleration vector!'];
            cont3 = cont3+1;
        end
    end
end

FileTemperature = ['' IDcentralina '-' DTcatena '-TLH-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 TempDef_TLH(a,b) > Tmax || TempDef_TLH(a,b) < Tmin
            cont2 = cont2+1;
            if b == 1
                if isfile(FileTemperature) == 1
                    DatiRaw = csvread(RawDataFile);
                    [rDR,cDR] = size(DatiRaw);
                    RawDate = find(DatiRaw(:,1)<=datenum(datainiTLH));
                    if isempty(RawDate) == 1
                        cc = 2;
                        while cc <= c
                            if TempDef_TLH(a,cc) > Tmax || TempDef_TLH(a,cc) < Tmin
                                cc = cc+1;
                            else
                                break
                            end
                        end
                        TempDef_TLH(a,b) = TempDef_TLH(a,cc);
                    else
                        if isnan(DatiRaw(RawDate(end),a+1)) == 0
                            TempDef_TLH(a,b) = DatiRaw(RawDate(end),a+1);
                            ErrTiltLinkH(1,a) = 0.5;
                            wardat = 'Temperature data of Tilt Link H nodes corrected using Raw Data of reference Csv file.';
                            fprintf(fileID,fmt,wardat);
                        else
                            cc = 2;
                            while cc <= c
                                if TempDef_TLH(a,cc) > Tmax || TempDef_TLH(a,cc) < Tmin
                                    cc = cc+1;
                                else
                                    break
                                end
                            end
                            TempDef_TLH(a,b) = TempDef_TLH(a,cc);
                        end
                    end
                else
                    cc = 2;
                    while cc <= c
                        if TempDef_TLH(a,cc) > Tmax || TempDef_TLH(a,cc) < Tmin
                            cc = cc+1;
                        else
                            break
                        end
                    end
                    TempDef_TLH(a,b) = TempDef_TLH(a,cc);
                end
            else
                TempDef_TLH(a,b) = TempDef_TLH(a,b-1);
                dY_TLH(i,j-1) = 0;
                dZ_TLH(i,j-1) = 0;
                ErrTiltLinkH(1,a) = 0.5;
            end
            textT = ['' num2str(cont2) ' correction executed for Tilt Link H - Temperature filter!'];
        end
    end
end

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

fprintf(fileID,fmt,textA);
fprintf(fileID,fmt,textA2);
fprintf(fileID,fmt,textT);
fclose(fileID);

[dX_TLH,dY_TLH,dZ_TLH] = azzeramenti_TLH(IDcentralina,DTcatena,dX_TLH,...
    dY_TLH,dZ_TLH,NodoTiltLinkH,FileName);

%% Filtro
[dX_TLH,dY_TLH,dZ_TLH] = filtro_TLH(dX_TLH,dY_TLH,dZ_TLH,Ndevst,Wdevst,FileName);
fileID = fopen(FileName,'a');
fprintf(fileID,fmt,text);
if NuovoZeroTLH == 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
    dX_TLH = dX_TLH(:,ini:end);
    dY_TLH = dY_TLH(:,ini:end);
    dZ_TLH = dZ_TLH(:,ini:end);
    TempDef_TLH = TempDef_TLH(ini:end,:);
    DatiElabTiltLinkH = DatiElabTiltLinkH(ini:end,:);
    ARRAYdateTLH = ARRAYdateTLH(ini:end,1); 
    ErrTiltLinkH = ErrTiltLinkH(ini:end,:);
end

%% Finalizzo i calcoli
[rx,cx] = size(dZ_TLH);
% totale del singolo nodo data per data
sommaX_TLH = zeros(rx,cx+1);
sommaY_TLH = zeros(rx,cx+1);
sommaZ_TLH = zeros(rx,cx+1);
% cumulata del singolo nodo data per data in posizione assoluta
% il primo valore è quello di posizione assoluta iniziale
Y_TLH = zeros(rx,cx+1);
Z_TLH = zeros(rx,cx+1);
% cumulata del singolo nodo data per data in posizione relativa
% il primo valore è quello di posizione assoluta iniziale
Xlocal_TLH = zeros(Nnodi,Ndati);
Ylocal_TLH = zeros(Nnodi,Ndati);
Zlocal_TLH = zeros(Nnodi,Ndati);
Speed_TLH = zeros(Nnodi,Ndati);
Speed_local_TLH = zeros(Nnodi,Ndati);
Acceleration_TLH = zeros(Nnodi,Ndati);
Acceleration_local_TLH = zeros(Nnodi,Ndati);

if NuovoZeroTLH == 1
    [rE,cE] = size(DatiElabTiltLinkH);
    cont = 4;
    n = 1;
    while cont<=cE
        sommaX_TLH(n,1) = cell2mat(DatiElabTiltLinkH(1,cont-1))';
        Xlocal_TLH(n,1) = sommaX_TLH(n,1);
        sommaY_TLH(n,1) = cell2mat(DatiElabTiltLinkH(1,cont))';
        Ylocal_TLH(n,1) = sommaY_TLH(n,1);
        sommaZ_TLH(n,1) = cell2mat(DatiElabTiltLinkH(1,cont+1))';
        Zlocal_TLH(n,1) = sommaZ_TLH(n,1);
        Y_TLH(n,1) = cell2mat(DatiElabTiltLinkH(1,cont+3))';
        Z_TLH(n,1) = cell2mat(DatiElabTiltLinkH(1,cont+4))';
        Speed_local_TLH(n,1:rE) = cell2mat(DatiElabTiltLinkH(:,cont+7))';
        Speed_TLH(n,1:rE) = cell2mat(DatiElabTiltLinkH(:,cont+6))';
        Acceleration_local_TLH(n,1:rE) = cell2mat(DatiElabTiltLinkH(:,cont+9))';
        Acceleration_TLH(n,1:rE) = cell2mat(DatiElabTiltLinkH(:,cont+8))';
        cont = cont+13;
        n = n+1;
    end
else
    % Primo dato
    Y_TLH(:,1) = PsTLH;
    Z_TLH(:,1) = 0;
end
for iii = 1:cx % date
    % somma singoli spostamenti del singolo nodo
    sommaX_TLH(:,iii+1) = sum(dX_TLH(:,1:iii),2)+sommaX_TLH(:,1);
    sommaY_TLH(:,iii+1) = sum(dY_TLH(:,1:iii),2)+sommaY_TLH(:,1);
    sommaZ_TLH(:,iii+1) = sum(dZ_TLH(:,1:iii),2)+sommaZ_TLH(:,1);
    % spostamenti locali riferiti allo 0
    Xlocal_TLH(:,iii+1) = sommaX_TLH(:,iii+1);
    Ylocal_TLH(:,iii+1) = sommaY_TLH(:,iii+1);
    Zlocal_TLH(:,iii+1) = sommaZ_TLH(:,iii+1);
    % cumulata spostamenti
    Y_TLH(:,iii+1) = Y_TLH(:,1) + cumsum(sommaY_TLH(:,iii+1)) - cumsum(sommaY_TLH(:,1));
    Z_TLH(:,iii+1) = Z_TLH(:,1) + cumsum(sommaZ_TLH(:,iii+1)) - cumsum(sommaZ_TLH(:,1));
end

%% Calcolo velocità di spostamento giornaliera
[numDate,~] = size(ARRAYdateTLH); % 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 = ARRAYdateTLH(d) - ARRAYdateTLH(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:rTLH
        N = 1;
        for dd = 1:nDate
            Speed_TLH(s,ContSUP(N,1)) = (Z_TLH(s,ContSUP(N,1))-Z_TLH(s,ContINF(N,1)))/(ARRAYdateTLH(ContSUP(N,1))-ARRAYdateTLH(ContINF(N,1)));
            Speed_local_TLH(s,ContSUP(N,1)) = (Zlocal_TLH(s,ContSUP(N,1))-Zlocal_TLH(s,ContINF(N,1)))/(ARRAYdateTLH(ContSUP(N,1))-ARRAYdateTLH(ContINF(N,1)));
            Acceleration_TLH(s,ContSUP(N,1)) = (Speed_TLH(s,ContSUP(N,1))-Speed_TLH(s,ContINF(N,1)))/(ARRAYdateTLH(ContSUP(N,1))-ARRAYdateTLH(ContINF(N,1)));
            Acceleration_local_TLH(s,ContSUP(N,1)) = (Speed_local_TLH(s,ContSUP(N,1))-Speed_local_TLH(s,ContINF(N,1)))/(ARRAYdateTLH(ContSUP(N,1))-ARRAYdateTLH(ContINF(N,1)));
            N = N+1;
        end
    end
end
    
fclose(fileID);
% Approssimo i dati con il corretto numero di cifre decimali
[Y_TLH,Z_TLH,Ylocal_TLH,Zlocal_TLH,Speed_TLH,Speed_local_TLH,...
    Acceleration_TLH,Acceleration_local_TLH,TempDef_TLH] = approx_TLH(...
    Y_TLH,Z_TLH,Ylocal_TLH,Zlocal_TLH,Speed_TLH,Speed_local_TLH,...
    Acceleration_TLH,Acceleration_local_TLH,TempDef_TLH,FileName);

% Riordino matrice errori
[r,~] = size(ErrTiltLinkH);
Matrice_err = zeros(r,rTLH);
for i = 1:r % date
    d = 1;
    for n = 1:rTLH % nodi
        j = 1;
        err = ErrTiltLinkH(i,d:d+3);
        while j <= 4
            if err(1,j) == 1
                Matrice_err(i,n) = 1;
            end
            j = j+1;
        end
        d = d+4;
    end
end
ErrTiltLinkH = Matrice_err';
    
text = 'Tilt Link H calculation executed correctly';
fileID = fopen(FileName,'a');
fmt = '%s \r';
fprintf(fileID,fmt,text);
fclose(fileID);

end