% 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 [accRL,ris_acc_RL,T_RL] = conv_grezziRL(rRL,accRL,tempRL,DCalRLTot,MEMS,FileName)

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

if MEMS == 2
    % Spacchetto
    caX_RL = DCalRLTot(:,1);
    caY_RL = DCalRLTot(:,4);
    caZ_RL = DCalRLTot(:,7);
    pIntX_RL = DCalRLTot(:,2);
    pIntY_RL = DCalRLTot(:,5);
    pIntZ_RL = DCalRLTot(:,8);
    iIntX_RL = DCalRLTot(:,3);
    iIntY_RL = DCalRLTot(:,6);
    iIntZ_RL = DCalRLTot(:,9);
    caT_RL = DCalRLTot(:,10);
    intT_RL = DCalRLTot(:,11);
    
    %% Accelerometri
    cont = 1;
    cn = 1;
    % Contatore dei nodi, corregge le accelerazioni con le calibrazioni
    for ii=1:3*rRL
        if cont==1
            accRL(:,ii) = accRL(:,ii)*caX_RL(cn)+(tempRL(:,cn)*pIntX_RL(cn)+iIntX_RL(cn));
            cont = cont+1;
        elseif cont==2
            accRL(:,ii) = accRL(:,ii)*caY_RL(cn)+(tempRL(:,cn)*pIntY_RL(cn)+iIntY_RL(cn));
            cont = cont+1;
        else
            accRL(:,ii) = accRL(:,ii)*caZ_RL(cn)+(tempRL(:,cn)*pIntZ_RL(cn)+iIntZ_RL(cn));
            cont = 1;
            cn = cn+1;
        end
    end
    
    %% Conversione delle temperature
    [rT,~] = size(tempRL);
    T_RL = zeros(rT,1);
    
    for t = 1:rRL
        T_RL(:,t) = tempRL(:,t)*caT_RL(t,1) + intT_RL(t,1);
    end
end

%% Risultanti
[rAcc,cAcc] = size(accRL);
ris_acc_RL = zeros(rAcc,cAcc/3); % matrice risultante accelerazioni

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_RL(i,cont) = (accRL(i,cn*3+1)^2+accRL(i,cn*3+2)^2+accRL(i,cn*3+3)^2)^0.5;
    end
    cn = cn+1;
    cont = cont+1;
end

text = 'Raw Data of Radial Link converted into physycal units correctly. conv_grezziRL function closed';
fprintf(fileID,fmt,text);
fclose(fileID);

end