% 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,magRL,ris_acc_RL,ris_mag_RL,T_RL] = conv_grezziRL_Dati(...
    rRL,accRL,magRL,tempRL,DCalRLTot,FileName)

% 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);

%% Magnetometri
for ii=1:3*rRL
    magRL(:,ii) = magRL(:,ii)/1000; % 1000 Gauss
end
clear ii

%% 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

%% Calcolo della risultante
[rAcc,cAcc] = size(accRL);
[rMag,cMag] = size(magRL);

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

clear i
clear ii
clear cont
clear cn
cont = 1; % contatore
cn = 0;
n = 1;

for ii = 1:cAcc % colonne
    if n == 4
        n = 1;
    end
    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;
        ris_mag_RL(i,cont) = (magRL(i,cn*3+1)^2+magRL(i,cn*3+2)^2+magRL(i,cn*3+3)^2)^0.5;
    end
    if n == 3
        cn = cn+1;
        cont = cont+1;
    end
    n = n+1;
end

text = 'Raw Data of Radial Link converted into physycal units correctly';
fileID = fopen(FileName,'a');
fmt = '%s \r';
fprintf(fileID,fmt,text);
fclose(fileID);

end