% 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 [accIPL,magIPL,ris_acc_IPL,ris_mag_IPL,tempIPL,ErrInPlaceLink] = conv_grezziIPL(IDcentralina,rIPL,...
    accIPL,magIPL,tempIPL,DCalIPLTot,tolleranzaAcc,ErrInPlaceLink,NodoInPlaceLink,MEMS,FileName)

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

if MEMS == 1 || MEMS == 2
    
    caX = DCalIPLTot(:,1);
    caY = DCalIPLTot(:,4);
    caZ = DCalIPLTot(:,7);
    pIntX = DCalIPLTot(:,2);
    pIntY = DCalIPLTot(:,5);
    pIntZ = DCalIPLTot(:,8);
    iIntX = DCalIPLTot(:,3);
    iIntY = DCalIPLTot(:,6);
    iIntZ = DCalIPLTot(:,9);
    caT = DCalIPLTot(:,10);
    intT = DCalIPLTot(:,11);
    MagX = DCalIPLTot(:,12);
    MagY = DCalIPLTot(:,13);
    MagZ = DCalIPLTot(:,14);
    
    %% Magnetometri
    % controllo dei dati di campo magnetico per anomalie modello Baveno
    [num,~] = size(magIPL);
    mR = 1;
    t = 1;
    Err = 1;
    while mR <= 3*rIPL
        for mC = 1:num
            if mC == 1
                if magIPL(mC,mR)==0 && magIPL(mC,mR+1)==0 && magIPL(mC,mR+2)==0
                    primo = 0;
                    letMag = 2;
                    while primo == 0
                        if letMag > num
                            break
                        end
                        if magIPL(letMag,mR)==0 && magIPL(letMag,mR+1)==0 && magIPL(letMag,mR+2)==0
                            letMag = letMag+1;
                        else
                            magIPL(mC,mR:mR+2) = magIPL(letMag,mR:mR+2);
                            accIPL(mC,mR:mR+2) = accIPL(letMag,mR:mR+2);
                            tempIPL(mC,t) = tempIPL(letMag,t);
                            ErrInPlaceLink(mC,Err:Err+6) = 1;
                            primo = 1;
                        end
                    end
                end
            else
                if magIPL(mC,mR) == 0 && magIPL(mC,mR+1) == 0 && magIPL(mC,mR+2) == 0
                    magIPL(mC,mR:mR+2) = magIPL(mC-1,mR:mR+2);
                    accIPL(mC,mR:mR+2) = accIPL(mC-1,mR:mR+2);
                    tempIPL(mC,t) = tempIPL(mC-1,t);
                    ErrInPlaceLink(mC,Err:Err+6) = 1;
                end
            end
        end
        mR = mR+3;
        t = t+1;
        Err = Err+7;
    end
elseif MEMS == 0 && strcmp(NodoInPlaceLink(1,4),'0-10 V') == 1
    ax = DCalIPLTot(:,1);
    bx = DCalIPLTot(:,2);
    cx = DCalIPLTot(:,3);
    ay = DCalIPLTot(:,4);
    by = DCalIPLTot(:,5);
    cy = DCalIPLTot(:,6);
    az = DCalIPLTot(:,7);
    bz = DCalIPLTot(:,8);
    cz = DCalIPLTot(:,9);
end

if strcmp(NodoInPlaceLink(1,4),'ADC') || strcmp(NodoInPlaceLink{1,4},'null') || ...
        isempty(NodoInPlaceLink{1,4}) == 1 || strcmp(NodoInPlaceLink(1,4),'g')  == 1
    check = isnan(MagX);
    if check == 0 % Applico gli offset
        cont = 1;
        cn = 1;
        for ii = 1:3*rIPL
            if cont==1
                magIPL(:,ii) = ((magIPL(:,ii) - MagX(cn))*100)/100000;
                cont = cont+1;
            elseif cont==2
                magIPL(:,ii) = ((magIPL(:,ii) - MagY(cn))*100)/100000;
                cont = cont+1;
            elseif cont==3
                magIPL(:,ii) = ((magIPL(:,ii) - MagZ(cn))*100)/100000;
                cont = 1;
                cn = cn+1;
            end
        end
    else
        magIPL = magIPL/1000; % 1000 Gauss
    end
    clear ii
elseif strcmp(NodoInPlaceLink(1,4),'Gradi') || strcmp(NodoInPlaceLink(1,4),'digit')
    magIPL = [];
end

if MEMS == 1 || MEMS == 2
    %% Accelerometri
    cont = 1;
    cn = 1;
    % Contatore dei nodi, corregge le accelerazioni con le calibrazioni
    for ii=1:3*rIPL
        if cont==1
            accIPL(:,ii) = accIPL(:,ii)*caX(cn)+(tempIPL(:,cn)*pIntX(cn)+iIntX(cn));
            cont = cont+1;
        elseif cont==2
            accIPL(:,ii) = accIPL(:,ii)*caY(cn)+(tempIPL(:,cn)*pIntY(cn)+iIntY(cn));
            cont = cont+1;
        else
            accIPL(:,ii) = accIPL(:,ii)*caZ(cn)+(tempIPL(:,cn)*pIntZ(cn)+iIntZ(cn));
            cont = 1;
            cn = cn+1;
        end
    end
    
    %% Conversione delle temperature
    for t = 1:rIPL
        tempIPL(:,t) = tempIPL(:,t)*caT(t,1) + intT(t,1);
    end
elseif MEMS == 0 && strcmp(NodoInPlaceLink(1,4),'0-10 V') == 1
    %% Accelerometri
    cont = 1;
    cn = 1;
    % Contatore dei nodi, corregge le accelerazioni con le calibrazioni
    for ii=1:3*rIPL
        if cont==1
            accIPL(:,ii) = ax(cn)*(accIPL(:,ii).^2)+bx(cn)*accIPL(:,ii)+cx(cn);
            cont = cont+1;
        elseif cont==2
            accIPL(:,ii) = ay(cn)*(accIPL(:,ii).^2)+by(cn)*accIPL(:,ii)+cy(cn);
            cont = cont+1;
        else
            accIPL(:,ii) = az(cn)*(accIPL(:,ii).^2)+bz(cn)*accIPL(:,ii)+cz(cn);
            cont = 1;
            cn = cn+1;
        end
    end
end

%% Risultanti
[rAcc,cAcc] = size(accIPL);
[rMag,cMag] = size(magIPL);

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

clear i
clear ii
clear cont
clear cn

cont = 1; % contatore
cn = 0;

if strcmp(NodoInPlaceLink(1,4),'ADC') || strcmp(NodoInPlaceLink{1,4},'null') || ...
        isempty(NodoInPlaceLink{1,4}) == 1 || strcmp(NodoInPlaceLink(1,4),'g')  == 1
    %% Calcolo della risultante
    for ii = 1:(cAcc/3) % colonne
        for i = 1:rAcc % righe
            ris_acc_IPL(i,cont) = (accIPL(i,cn*3+1)^2+accIPL(i,cn*3+2)^2+accIPL(i,cn*3+3)^2)^0.5;
            ris_mag_IPL(i,cont) = (magIPL(i,cn*3+1)^2+magIPL(i,cn*3+2)^2+magIPL(i,cn*3+3)^2)^0.5;
        end
        cn = cn+1;
        cont = cont+1;
    end

    [r,c] = size(ris_acc_IPL);
    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_IPL(i,j)-ris_acc_IPL(i-1,j)) > tolleranzaAcc
                accIPL(i,mmm) = accIPL(i-1,mmm);
                accIPL(i,mmm+1) = accIPL(i-1,mmm+1);
                accIPL(i,mmm+2) = accIPL(i-1,mmm+2);
                tempIPL(i,j) = tempIPL(i-1,j);
                ErrInPlaceLink(i,Err:Err+6) = 1;
            end
            if strcmp(IDcentralina,'ID0115') == 1
                if ris_acc_IPL(i,j) < 0.9 || ris_acc_IPL(i,j) > 1.17 % Il nodo è fuori taratura!
                    accIPL(i,mmm) = accIPL(i-1,mmm);
                    accIPL(i,mmm+1) = accIPL(i-1,mmm+1);
                    accIPL(i,mmm+2) = accIPL(i-1,mmm+2);
                    tempIPL(i,j) = tempIPL(i-1,j);
                    ErrInPlaceLink(i,Err:Err+6) = 1;
                end
            else
                if ris_acc_IPL(i,j) < 0.9 || ris_acc_IPL(i,j) > 1.12 % Il nodo è fuori taratura!
                    accIPL(i,mmm) = accIPL(i-1,mmm);
                    accIPL(i,mmm+1) = accIPL(i-1,mmm+1);
                    accIPL(i,mmm+2) = accIPL(i-1,mmm+2);
                    tempIPL(i,j) = tempIPL(i-1,j);
                    ErrInPlaceLink(i,Err:Err+6) = 1;
                end
            end
        end
        mmm = mmm+3;
        Err = Err+7;
    end
end

text = 'Raw Data of In Place Link converted into physical units correctly. conv_grezziTLH function started';
fprintf(fileID,fmt,text);
fclose(fileID);

end