% 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 [accTL,magTL,ris_acc,ris_mag,tempTL,ErrTiltLink] = conv_grezziTL(rTL,accTL,magTL,...
       tempTL,DCalTLTot,tolleranzaAcc,MEMS,ErrTiltLink,IDcentralina,DTcatena,FileName)

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

if MEMS == 1 || MEMS == 2
    caX = DCalTLTot(:,1);
    caY = DCalTLTot(:,4);
    caZ = DCalTLTot(:,7);
    pIntX = DCalTLTot(:,2);
    pIntY = DCalTLTot(:,5);
    pIntZ = DCalTLTot(:,8);
    iIntX = DCalTLTot(:,3);
    iIntY = DCalTLTot(:,6);
    iIntZ = DCalTLTot(:,9);
    caT = DCalTLTot(:,10);
    intT = DCalTLTot(:,11);
    MagX = DCalTLTot(:,12);
    MagY = DCalTLTot(:,13);
    MagZ = DCalTLTot(:,14);
elseif MEMS == 3
    MagX = DCalTLTot(:,1);
    MagY = DCalTLTot(:,2);
    MagZ = DCalTLTot(:,3);
end

if MEMS == 2
    %% Magnetometri
    % controllo dei dati di campo magnetico per anomalie modello Baveno
    [num,~] = size(magTL);
    mR = 1;
    Err = 1;
    t = 1;
    while mR <= 3*rTL
        for mC = 1:num
            if mC == 1
                if magTL(mC,mR)==0 && magTL(mC,mR+1)==0 || magTL(mC,mR)==0 && magTL(mC,mR+2)==0 ||...
                        magTL(mC,mR+1)==0 && magTL(mC,mR+2)==0
                    primo = 0;
                    letMag = 2;
                    while primo == 0
                        if letMag > num
                            break
                        end
                        if magTL(mC,mR)==0 && magTL(mC,mR+1)==0 || magTL(mC,mR)==0 && magTL(mC,mR+2)==0 ||...
                                magTL(mC,mR+1)==0 && magTL(mC,mR+2)==0
                            letMag = letMag+1;
                        else
                            magTL(mC,mR:mR+2) = magTL(letMag,mR:mR+2);
                            accTL(mC,mR:mR+2) = accTL(letMag,mR:mR+2);
                            tempTL(mC,t) = tempTL(letMag,t);
                            ErrTiltLink(mC,Err:Err+6) = 1;
                            primo = 1;
                        end
                    end
                end
            else
                if magTL(mC,mR)==0 && magTL(mC,mR+1)==0 || magTL(mC,mR)==0 && magTL(mC,mR+2)==0 ||...
                        magTL(mC,mR+1)==0 && magTL(mC,mR+2)==0
                    
                    magTL(mC,mR:mR+2) = magTL(mC-1,mR:mR+2);
                    accTL(mC,mR:mR+2) = accTL(mC-1,mR:mR+2);
                    tempTL(mC,t) = tempTL(mC-1,t);
                    ErrTiltLink(mC,Err:Err+6) = 1;
                end
            end
        end
        mR = mR+3;
        t = t+1;
        Err = Err+7;
    end
end

check = isnan(MagX);
if check == 0 % Applico gli offset
    cont = 1;
    cn = 1; 
    for ii = 1:3*rTL
        if cont==1
            if MEMS == 2
                magTL(:,ii) = ((magTL(:,ii) - MagX(cn))*100)/100000;
            elseif MEMS == 3
                magTL(:,ii) = (magTL(:,ii) - MagX(cn));
            end
           cont = cont+1;
        elseif cont==2
            if MEMS == 2
               magTL(:,ii) = ((magTL(:,ii) - MagY(cn))*100)/100000; 
            elseif MEMS == 3
                magTL(:,ii) = (magTL(:,ii) - MagY(cn));
            end
            cont = cont+1;
        elseif cont==3
            if MEMS == 2
               magTL(:,ii) = ((magTL(:,ii) - MagZ(cn))*100)/100000; 
            elseif MEMS == 3
                magTL(:,ii) = (magTL(:,ii) - MagZ(cn));
            end
            cont = 1;
            cn = cn+1;
        end
    end
elseif MEMS == 1 % MEMS vecchi (pre 21 Maggio 2016)
    for ii=1:3*rTL
        if mod(ii,3)==0
           magTL(:,ii) = magTL(:,ii)/980; % 980 Gauss
        else
            magTL(:,ii) = magTL(:,ii)/1100; % 1100 Gauss
        end
    end
elseif MEMS == 2 % MEMS nuovi (post 21 Maggio 2016)
       magTL = magTL/1000; % 1000 Gauss
elseif MEMS == 3
end
clear ii

%% Accelerometri
if MEMS == 1 || MEMS == 2
    cont = 1;
    cn = 1;
    % Contatore dei nodi, corregge le accelerazioni con le calibrazioni
    for ii=1:3*rTL
        if cont==1
            accTL(:,ii) = accTL(:,ii)*caX(cn)+(tempTL(:,cn)*pIntX(cn)+iIntX(cn));
            cont = cont+1;
        elseif cont==2
            accTL(:,ii) = accTL(:,ii)*caY(cn)+(tempTL(:,cn)*pIntY(cn)+iIntY(cn));
            cont = cont+1;
        else
            accTL(:,ii) = accTL(:,ii)*caZ(cn)+(tempTL(:,cn)*pIntZ(cn)+iIntZ(cn));
            cont = 1;
            cn = cn+1;
        end
    end

    %% Conversione delle temperature
    for t = 1:rTL
        tempTL(:,t) = tempTL(:,t)*caT(t,1) + intT(t,1);
    end
end

%% Risultanti
[rAcc,cAcc] = size(accTL);
[rMag,cMag] = size(magTL);

ris_acc = zeros(rAcc,cAcc/3); % matrice risultante accelerazioni
ris_mag = zeros(rMag,cMag/3); % matrice risultante campi magnetici

clear i
clear ii
clear cont
clear cn

cont = 1; % contatore
cn = 0;

%% Calcolo della risultante
for ii = 1:(cAcc/3) % colonne
    for i = 1:rAcc % righe
        ris_acc(i,cont) = (accTL(i,cn*3+1)^2+accTL(i,cn*3+2)^2+accTL(i,cn*3+3)^2)^0.5;
        ris_mag(i,cont) = (magTL(i,cn*3+1)^2+magTL(i,cn*3+2)^2+magTL(i,cn*3+3)^2)^0.5;
    end
    cn = cn+1;
    cont = cont+1;
end

%% Filtri sulla risultante
[r,c] = size(ris_acc);
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(i,j)-ris_acc(i-1,j)) > tolleranzaAcc
            accTL(i,mmm) = accTL(i-1,mmm);
            accTL(i,mmm+1) = accTL(i-1,mmm+1);
            accTL(i,mmm+2) = accTL(i-1,mmm+2);
            tempTL(i,j) = tempTL(i-1,j);
            ErrTiltLink(i,Err:Err+6) = 1;
        end
        if strcmp(IDcentralina,'ID0003') && strcmp(DTcatena,'DT0002')
        elseif ris_acc(i,j) < 0.9 || ris_acc(i,j) > 1.3 % Il nodo è fuori taratura! 
            accTL(i,mmm) = accTL(i-1,mmm);
            accTL(i,mmm+1) = accTL(i-1,mmm+1);
            accTL(i,mmm+2) = accTL(i-1,mmm+2);
            tempTL(i,j) = tempTL(i-1,j);
            ErrTiltLink(i,Err:Err+6) = 1;
        end
    end
    mmm = mmm+3;
    Err = Err+7;
end

text = 'Raw Data of Tilt Link V converted into physical units correctly. conv_grezziTL function closed';
fprintf(fileID,fmt,text);
fclose(fileID);

end