% Funzione che converte i dati grezzi in dati di accelerazione usando i 
% valori delle calibrazioni per i Tilt Link
% accTL raccoglie le accelerazioni
% magTL raccoglie i dati di campo magnetico

function [accTLH,ris_acc_TLH,tempTLH,ErrTiltLinkH] = conv_grezziTLH(rTLH,...
    accTLH,tempTLH,DCalTLHTot,tolleranzaAcc,MEMS,ErrTiltLinkH,FileName)

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

if MEMS == 1 || MEMS == 2
    caX = DCalTLHTot(:,1);
    caY = DCalTLHTot(:,4);
    caZ = DCalTLHTot(:,7);
    pIntX = DCalTLHTot(:,2);
    pIntY = DCalTLHTot(:,5);
    pIntZ = DCalTLHTot(:,8);
    iIntX = DCalTLHTot(:,3);
    iIntY = DCalTLHTot(:,6);
    iIntZ = DCalTLHTot(:,9);
    caT = DCalTLHTot(:,10);
    intT = DCalTLHTot(:,11);
    
    %% Accelerometri
    cont = 1;
    cn = 1;
    t = 1;
    % Contatore dei nodi, corregge le accelerazioni con le calibrazioni
    for ii=1:3*rTLH
        if cont==1
            accTLH(:,ii) = accTLH(:,ii)*caX(cn)+(tempTLH(:,cn)*pIntX(cn)+iIntX(cn));
            cont = cont+1;
        elseif cont==2
            accTLH(:,ii) = accTLH(:,ii)*caY(cn)+(tempTLH(:,cn)*pIntY(cn)+iIntY(cn));
            cont = cont+1;
        else
            accTLH(:,ii) = accTLH(:,ii)*caZ(cn)+(tempTLH(:,cn)*pIntZ(cn)+iIntZ(cn));
            cont = 1;
            cn = cn+1;
        end
    end
    
    %% Conversione delle temperature
    for t = 1:rTLH
        tempTLH(:,t) = tempTLH(:,t)*caT(t,1) + intT(t,1);
    end
    
    %% Calcolo della risultante
    [rAcc,cAcc] = size(accTLH);
    ris_acc_TLH = zeros(rAcc,cAcc/3); % matrice risultante accelerazioni
    clear i
    clear ii
    clear cont
    clear cn
    cont = 1; % contatore
    cn = 0;
    
    for ii = 1:(cAcc/3) % colonne
        for i = 1:rAcc % righe
            ris_acc_TLH(i,cont) = (accTLH(i,cn*3+1)^2+accTLH(i,cn*3+2)^2+accTLH(i,cn*3+3)^2)^0.5;
        end
        cn = cn+1;
        cont = cont+1;
    end
    
    %% Filtri sulla risultante
    [r,c] = size(ris_acc_TLH);
    mmm = 1;
    Err = 1;
    for j = 1:c % Nodi
        for i = 2:r % Letture
            % se il valore assoluto della differenza è maggiore della
            % tolleranza, pongo gli spostamenti giornalieri pari a 0
            if abs(ris_acc_TLH(i,j)-ris_acc_TLH(i-1,j)) > tolleranzaAcc
                accTLH(i,mmm) = accTLH(i-1,mmm);
                accTLH(i,mmm+1) = accTLH(i-1,mmm+1);
                accTLH(i,mmm+2) = accTLH(i-1,mmm+2);
                tempTLH(i,j) = tempTLH(i-1,j);
                ErrTiltLinkH(i,Err:Err+3) = 1;
            end
            if ris_acc_TLH(i,j) < 0.9 || ris_acc_TLH(i,j) > 1.1 % Il nodo è fuori taratura!
                accTLH(i,mmm) = accTLH(i-1,mmm);
                accTLH(i,mmm+1) = accTLH(i-1,mmm+1);
                accTLH(i,mmm+2) = accTLH(i-1,mmm+2);
                tempTLH(i,j) = tempTLH(i-1,j);
                ErrTiltLinkH(i,Err:Err+3) = 1;
            end
        end
        mmm = mmm+3;
        Err = Err+4;
    end
else
    ris_acc_TLH = [];
end

text = 'Raw Data of Tilt Link H converted into physical units correctly. conv_grezziTLH function ended';
fprintf(fileID,fmt,text);
fclose(fileID);

end