% Adaptação dos angulos solicitados ao que os motores requerem
%
% pos_final=adaptate(scuid,position, tipo)
%
% scuid    -> ID do slave
% position -> Posição solicitada (para os tres servos)
% tipo     ->   valor para enviar (tipo =  1)
%               valor para ler    (tipo = -1)
% pos_final-> Posição a aplicar aos servos
%
% É executado a conversão de sinais, introdução de offsets constantes e relações de
% transmissão mais os offsets variáveis

function pos_final=adapt(scuid,position,tipo)
mlock


persistent calib

scu_max = 8;

% Preenchimento de algum argumento não fornecido
if (nargin<3)
    tipo=1;
end

% Desvios de posição de cada unidade de controlo
if(isempty(calib))
    if(exist('c:/mATLAB7/toolbox/comm2/matriz_calib.mat', 'file'))
        disp('existe')
        load('c:/mATLAB7/toolbox/comm2/matriz_calib.mat', 'calib');
    else
        calib=[0   0   0;       % SCU 1: Perna direita  (tornozelo + pé + joelho)
               0   0   0;       % SCU 2: Perna esquerda (tornozelo + pé + joelho)
               0   0   0;       % SCU 3: Anca direita   (mov. dianteiro + lateral + rotativo)
               0   0   0;       % SCU 4: Anca esquerdo  (mov. dianteiro + lateral + rotativo)
               0   0   0;       % SCU 5: Tronco
               0   0   0;       % SCU 6: Braço direito
               0   0   0;       % SCU 7: Braço esquerdo
               0   0   0];      % SCU 8: Cabeça
        save('c:/mATLAB7/toolbox/comm2/matriz_calib.mat', 'calib');
    end
end

        
if(nargin==0)
    disp(calib)
    return
end

% Sinal a adicionar as posições solicitadas
signal=[+1 +1 -1;
        -1 -1 +1;
        -1 -1 +1;
        +1 +1 -1];

% Offset a adicionar as posições solicitadas
offset=[   0   0 -70;
           0   0 +70;
           0   0   0;
           0   0   0];
       calib;
       
Nrel = [2.5     2.5     2.5
        2.5     2.5     2.5
        2.5     3.75    1
        2.5     3.75    1
        1       1       1
        1       1       1
        1       1       1
        1       1       1];
        
        
if(nargin == 1)

    if(length(scuid) == 1)
        H = scuid;
        for scu = 1:scu_max,
            calib(scu,:) = readjoint(H,scu,0)-offset(scu,:);
        end
    else
        % esta operação não reflecte, na matriz calib, o valor máximo de actuação!!
        calib = round(calib+scuid.*signal(1:4,:).*Nrel(1:4,:));
    end
        save('c:/mATLAB7/toolbox/comm2/matriz_calib.mat', 'calib');
        disp('matriz de calibração ajustada com sucesso')
        disp(calib);
        return
        
end

% Validação dos argumentos
% if (~isempty(max(scuid<1)) || size(position,2)>min(size(signal,2),size(offset,2)))
%     error('limit fcn:invalid arg','Argumentos inválidos!');
% end



N=size(position,1);
if(isempty(scuid))
    scuid = 1:N;
else
    if(length(scuid) ~= N)
        error('tamanho do scuid incompativel com a matriz position');
    end
end


pos_final=zeros(N,3);

% calib = [   10  30  5
%             0   -10  10   
%             -10   -35 -15 
%             10   25  0   ];


for i=1:N
    scu = scuid(i);
    if (tipo>0)
        % Adapatção da posição solicitada para aplicação aos motores
            pos_final(i,:) = position(i,:).*signal(scu,:).*Nrel(scu,:) + offset(scu,:)+calib(scu,:);
        % limitação das posições ao máximo permissivel
            if(max(abs(pos_final)) > 90), 
                pos(pos_final>90) = 90;
                pos(pos_final<-90) = -90;
                disp('posição limitada') 
                disp(pos_final)
            end
    else
        % Adaptação da posição lido nos motores para a das juntas
        pos_final(i,:) = (position(i,:)-offset(scu,:)).*signal(scu,:)./Nrel(scu,:)-calib(scu,:);
    end
end

pos_final = round(pos_final);