% Funzione che converte i dati grezzi in dati di accelerazione usando i 
% valori delle calibrazioni per i PreConv Link
% accPCL raccoglie le accelerazioni
% magPCL raccoglie i dati di campo magnetico
% ris_acc_PCL raccoglie le risultanti delle accelerazioni
% ris_mag_PCL raccoglie le risultanti dei campi magnetici
% T_PCL raccoglie i dati di temperatura

function [accPCL,ris_acc_PCL,T_PCL] = conv_grezziPCL(rPCL,accPCL,tempPCL,DCalPCLTot,MEMS,FileName)

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

if MEMS == 2
    % Spacchetto
    caX_PCL = DCalPCLTot(:,1);
    caY_PCL = DCalPCLTot(:,4);
    caZ_PCL = DCalPCLTot(:,7);
    pIntX_PCL = DCalPCLTot(:,2);
    pIntY_PCL = DCalPCLTot(:,5);
    pIntZ_PCL = DCalPCLTot(:,8);
    iIntX_PCL = DCalPCLTot(:,3);
    iIntY_PCL = DCalPCLTot(:,6);
    iIntZ_PCL = DCalPCLTot(:,9);
    caT_PCL = DCalPCLTot(:,10);
    intT_PCL = DCalPCLTot(:,11);
    
    %% Accelerometri
    cont = 1;
    cn = 1;
    % Contatore dei nodi, corregge le accelerazioni con le calibrazioni
    for ii=1:3*rPCL
        if cont==1
            accPCL(:,ii) = accPCL(:,ii)*caX_PCL(cn)+(tempPCL(:,cn)*pIntX_PCL(cn)+iIntX_PCL(cn));
            cont = cont+1;
        elseif cont==2
            accPCL(:,ii) = accPCL(:,ii)*caY_PCL(cn)+(tempPCL(:,cn)*pIntY_PCL(cn)+iIntY_PCL(cn));
            cont = cont+1;
        else
            accPCL(:,ii) = accPCL(:,ii)*caZ_PCL(cn)+(tempPCL(:,cn)*pIntZ_PCL(cn)+iIntZ_PCL(cn));
            cont = 1;
            cn = cn+1;
        end
    end
    
    %% Conversione delle temperature
    [rT,~] = size(tempPCL);
    T_PCL = zeros(rT,1);
    for t = 1:rPCL
        T_PCL(:,t) = tempPCL(:,t)*caT_PCL(t,1) + intT_PCL(t,1);
    end
end

%% Calcolo della risultante
[rAcc,cAcc] = size(accPCL);
ris_acc_PCL = 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_PCL(i,cont) = (accPCL(i,cn*3+1)^2+accPCL(i,cn*3+2)^2+accPCL(i,cn*3+3)^2)^0.5;
    end
    cn = cn+1;
    cont = cont+1;
end

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