% Funzione che converte i dati grezzi in dati di accelerazione usando i 
% valori delle calibrazioni per i RSN Link
% accRSN raccoglie le accelerazioni

function [accRSN,ris_acc,tempRSN] = conv_grezziRSN(rRSN,accRSN,...
    tempRSN,DCalRSNTot,MEMS,FileName)

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

if MEMS == 2 % RSN Link 1.0
    gainX = DCalRSNTot(:,1);
    caX = DCalRSNTot(:,2);
    intX = DCalRSNTot(:,3);
    caASSX = DCalRSNTot(:,4);
    intASSX = DCalRSNTot(:,5);
    gainY = DCalRSNTot(:,6);
    caY = DCalRSNTot(:,7);
    intY = DCalRSNTot(:,8);
    caASSY = DCalRSNTot(:,9);
    intASSY = DCalRSNTot(:,10);
    gainZ = DCalRSNTot(:,11);
    caZ = DCalRSNTot(:,12);
    intZ = DCalRSNTot(:,13);
    % caASSZ = DCalRSNTot(:,14); --> non necessari
    % intASSZ = DCalRSNTot(:,15); --> non necessari
    caT = DCalRSNTot(:,16);
    intT = DCalRSNTot(:,17);

    [righeREL,~] = size(accRSN);
    angREL_RSN = zeros(righeREL,rRSN*2);
    
    %% Accelerometri
    cont = 1;
    cn = 1;
    % Contatore dei nodi, corregge le accelerazioni con le calibrazioni
    for ii=1:3*rRSN
        if cont==1
            accRSN(:,ii) = accRSN(:,ii)*gainX(cn)+(tempRSN(:,cn)*caX(cn)+intX(cn));
            cont = cont+1;
        elseif cont==2
            accRSN(:,ii) = accRSN(:,ii)*gainY(cn)+(tempRSN(:,cn)*caY(cn)+intY(cn));
            cont = cont+1;
        else
            accRSN(:,ii) = accRSN(:,ii)*gainZ(cn)+(tempRSN(:,cn)*caZ(cn)+intZ(cn));
            cont = 1;
            cn = cn+1;
        end
    end

    % Calcolo e correggo angoli relativi in base al segno dell'asse Z
    cont = 1;
    contACC = 1;
    for ii=1:3*rRSN
        if cont==1 % X
            angREL_RSN(:,contACC) = real(asind(accRSN(:,ii))); %calcolo angolo relativo
            if angREL_RSN(:,contACC)>0 %check quando angolo relativo > 0
                if accRSN(:,ii+2)<0 % check quando asse z < 0 --> il montante si è capovolto!
                    angREL_RSN(:,contACC) = 180 - angREL_RSN(:,contACC);
                end
            elseif angREL_RSN(:,ii)<0 %check quando angolo relativo < 0
                if accRSN(:,ii+2)<0  % check quando asse z < 0 --> il montante si è capovolto!
                    angREL_RSN(:,contACC) = -180 - angREL_RSN(:,contACC);
                end
            end
            cont = cont+1;
            contACC = contACC+1;
        elseif cont==2 % Y
            angREL_RSN(:,contACC) = real(asind(accRSN(:,ii))); %calcolo angolo relativo
            if angREL_RSN(:,contACC)>0 %check quando angolo relativo > 0
                if accRSN(:,ii+1)<0 % check quando asse z < 0 --> il montante si è capovolto!
                    angREL_RSN(:,contACC) = 180 - angREL_RSN(:,contACC);
                end
            elseif angREL_RSN(:,ii)<0 %check quando angolo relativo < 0
                if accRSN(:,ii+1)<0  % check quando asse z < 0 --> il montante si è capovolto!
                    angREL_RSN(:,contACC) = -180 - angREL_RSN(:,contACC);
                end
            end
            cont = cont+1;
            contACC = contACC+1;
        else
            cont = 1;
        end
    end
    
    % Calcolo angoli assoluti
    cont = 1;
    cn = 1;
    for ii=1:2*rRSN
        if cont==1
            accRSN(:,ii) = angREL_RSN(:,ii)*caASSX(cn)+intASSX(cn);
            cont = cont+1;
        elseif cont==2
            accRSN(:,ii) = angREL_RSN(:,ii)*caASSY(cn)+intASSY(cn);
            cont = cont+1;
        else
            cont = 1;
            cn = cn+1;
        end
    end
    
    %% Conversione delle temperature
    [rT,~] = size(tempRSN);
    T_RSN = zeros(rT,1);
    
    for t = 1:rRSN
        T_RSN(:,t) = tempRSN(:,t)*caT(t,1) + intT(t,1);
    end
    
    %% Risultanti
    [rAcc,cAcc] = size(accRSN);
    ris_acc = zeros(rAcc,cAcc/3); % matrice risultante accelerazioni
    
    clear i
    clear ii
    clear cont
    clear cn
    cont = 1; % contatore
    cn = 0;
    n = 1;
    
    %% Calcolo della risultante
    for ii = 1:cAcc % colonne
        if n == 4
            n = 1;
        end
        for i = 1:rAcc % righe
            ris_acc(i,cont) = (accRSN(i,cn*3+1)^2+accRSN(i,cn*3+2)^2+accRSN(i,cn*3+3)^2)^0.5;
        end
        if n == 3
            cn = cn+1;
            cont = cont+1;
        end
        n = n+1;
    end
elseif MEMS == 3 % Murata
    %% Risultanti
    [rAcc,cAcc] = size(accRSN);
    ris_acc = zeros(rAcc,cAcc/3); % matrice risultante accelerazioni
    cont = 1; % contatore
    cn = 0;
    n = 1;
    
    %% Calcolo della risultante
    for ii = 1:cAcc % colonne
        if n == 4
            n = 1;
        end
        for i = 1:rAcc % righe
            ris_acc(i,cont) = (sind(accRSN(i,cn*3+1))^2+sind(accRSN(i,cn*3+2))^2+sind(accRSN(i,cn*3+3))^2)^0.5;
        end
        if n == 3
            cn = cn+1;
            cont = cont+1;
        end
        n = n+1;
    end
else
    ris_acc = [];
end

text = 'Raw Data of RSN Link converted into physycal units correctly. conv_grezziRSN function ended';
fprintf(fileID,fmt,text);
fclose(fileID);

end