% Funzione che converte i dati grezzi dei Klino Link in dati di
% angoli

function [ang_KL,temp_KL,ris_acc,ErrKlinoLink] = conv_grezziKL(ang_KL,temp_KL,...
    rKL,DCalKLTot,IDcentralina,DTcatena,NodoKlinoLink,ErrKlinoLink,FileName)

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

if strcmp(IDcentralina,'ID0231') == 1 && strcmp(DTcatena,'DT0251') == 1 
    angoloKL = real(asind(ang_KL(:,1:3))); %calcolo angolo relativo
    ang_KL(:,1:3) = angoloKL;
    
    %% Accelerometri
    cont = 1; % contatore
    cn = 0;
    [rAcc,cAcc] = size(ang_KL);
    ris_acc = zeros(rAcc,cAcc/3); % matrice risultante accelerazioni
    
    for ii = 1:(cAcc/3) % colonne
        for i = 1:rAcc % righe
            ris_acc(i,cont) = (ang_KL(i,cn*3+1)^2+ang_KL(i,cn*3+2)^2+ang_KL(i,cn*3+3)^2)^0.5;
        end
        cn = cn+1;
        cont = cont+1;
    end
    ris_acc = sind(ris_acc);
        
elseif strcmp(NodoKlinoLink(1,4),'ADC') || strcmp(NodoKlinoLink{1,4} ,'null') ...
        || isempty(NodoKlinoLink{1,4}) == 1 || strcmp(NodoKlinoLink(1,4),'g')
    
    if strcmp(NodoKlinoLink(1,4),'ADC') || strcmp(NodoKlinoLink{1,4} ,'null') ...
            || isempty(NodoKlinoLink{1,4}) == 1
        caX = DCalKLTot(:,1);
        caY = DCalKLTot(:,4);
        caZ = DCalKLTot(:,7);
        pIntX = DCalKLTot(:,2);
        pIntY = DCalKLTot(:,5);
        pIntZ = DCalKLTot(:,8);
        iIntX = DCalKLTot(:,3);
        iIntY = DCalKLTot(:,6);
        iIntZ = DCalKLTot(:,9);
        caT = DCalKLTot(:,10);
        intT = DCalKLTot(:,11);
    end

    if strcmp(NodoKlinoLink(1,4),'g') == 0
        %% Accelerometri
        cont = 1;
        cn = 1;
        n = 3;
        % Contatore dei nodi, corregge le accelerazioni con le calibrazioni
        for ii=1:n*rKL
            if cont==1
                ang_KL(:,ii) = ang_KL(:,ii)*caX(cn)+(temp_KL(:,cn)*pIntX(cn)+iIntX(cn));
                cont = cont+1;
            elseif cont==2
                ang_KL(:,ii) = ang_KL(:,ii)*caY(cn)+(temp_KL(:,cn)*pIntY(cn)+iIntY(cn));
                cont = cont+1;
            else
                ang_KL(:,ii) = ang_KL(:,ii)*caZ(cn)+(temp_KL(:,cn)*pIntZ(cn)+iIntZ(cn));
                cont = 1;
                cn = cn+1;
            end
        end
    end

    angoloKL = real(asind(ang_KL)); %calcolo angolo relativo
   
%     % Calcolo e correggo angoli relativi in base al segno dell'asse Z
%     cont = 1;
%     contACC = 1;
%     angoloKL = ang_KL;
%     for ii=1:3*rKL
%         if cont == 1 % X
%             angoloKL(:,contACC) = real(asind(ang_KL(:,ii))); %calcolo angolo relativo
%             if angoloKL(:,contACC) > 0 %check quando angolo relativo > 0
%                 if ang_KL(:,ii+2) < 0 % check quando asse z < 0 --> il montante si è capovolto!
%                     angoloKL(:,contACC) = pi - angoloKL(:,contACC);
%                 end
%             elseif angoloKL(:,ii) < 0 % check quando angolo relativo < 0
%                 if ang_KL(:,ii+2) < 0  % check quando asse z < 0 --> il montante si è capovolto!
%                     angoloKL(:,contACC) = -pi - angoloKL(:,contACC);
%                 end
%             end
%             cont = cont+1;
%             contACC = contACC+1;
%         elseif cont == 2 % Y
%             angoloKL(:,contACC) = real(asind(ang_KL(:,ii))); % calcolo angolo relativo
%             if angoloKL(:,contACC) > 0 %check quando angolo relativo > 0
%                 if ang_KL(:,ii+1) < 0 % check quando asse z < 0 --> il montante si è capovolto!
%                     angoloKL(:,contACC) = pi - angoloKL(:,contACC);
%                 end
%             elseif angoloKL(:,ii) < 0 %check quando angolo relativo < 0
%                 if ang_KL(:,ii+1) < 0  % check quando asse z < 0 --> il montante si è capovolto!
%                     angoloKL(:,contACC) = -pi - angoloKL(:,contACC);
%                 end
%             end
%             cont = cont+1;
%             contACC = contACC+1;
%         elseif cont == 3 % Z
%             angoloKL(:,contACC) = real(asind(ang_KL(:,ii))); % calcolo angolo relativo
%             cont = 1;
%             contACC = contACC+1;
%         end
%     end

    %% Conversione delle temperature
    if strcmp(NodoKlinoLink(1,4),'g') == 0
        for t = 1:rKL
            temp_KL(:,t) = temp_KL(:,t)*caT(t,1) + intT(t,1);
        end
    end

    %% Risultanti
    clear ii
    clear cont
    clear cn
    cont = 1; % contatore
    cn = 0;
    [rAcc,cAcc] = size(ang_KL);
    ris_acc = zeros(rAcc,cAcc/3); % matrice risultante accelerazioni
    
    for ii = 1:(cAcc/3) % colonne
        for i = 1:rAcc % righe
            ris_acc(i,cont) = (ang_KL(i,cn*3+1)^2+ang_KL(i,cn*3+2)^2+ang_KL(i,cn*3+3)^2)^0.5;
        end
        cn = cn+1;
        cont = cont+1;
    end
    
    ang_KL = angoloKL;
else
    ris_acc = [];
end

text = 'Raw Data of Klino Link converted into physical units correctly. conv_grezziKL function ended';
fprintf(fileID,fmt,text);
fclose(fileID);

end