% Adaptação dos angulos solicitados ao que os motores requerem
%
% pos_final=adapt(scuid,position)
%
% scuid    -> ID do slave
% position -> Posição solicitada (para os tres servos)
% pos_final-> Posição a aplicar aos servos
%
% Note que a correcção nem a limitação é executada.

function pos_final=adapt(scuid,position,servoact)

persistent calib

% 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;
          15   0 +70;
           0   0   0;
           0   0   0];

% Quantidade de SCUs
Nscu=size(offset,1);


% % Calibração
% if(nargin == 1)
%     H = scuid;
% 
%     % Extração das posições correntes
%     calib=zeros(Nscu,3);
%     for scu=1:Nscu
%         [calib(scu,:), st, rx, error,errorstr] = readjoint(H,scu,0);
%         if (error), break, end
%     end
%     
%     if (error==0)
%         calib=calib-offset                                 % Cálculo dos desvios
%         save('matriz_calib.mat','calib');                   % Armazenar matriz de calibração
%         disp('Matriz de calibração ajustada com sucesso')
%     else
%         disp('Calibração falhada!');
%     end
%     return
% end
% 
% % Desvios de posição de cada unidade de controlo
% if (isempty(calib))
%     if(exist('matriz_calib.mat','file'))
%         load('matriz_calib.mat','calib');
%     else
%         calib=zeros(Nscu,3);
%         save('matriz_calib.mat', 'calib');
%     end
% end

% Desvios de posição de cada unidade de controlo
calib=[ 15  25  25;       % SCU 1: Perna direedit ita  (tornozelo + pé + joelho)
         0  20   0;       % SCU 2: Perna esquerda (tornozelo + pé + joelho)
         0 -10 -15;       % SCU 3: Anca direita   (mov. dianteiro + lateral + rotativo)
         0   0   0];      % SCU 4: Anca esquerdo  (mov. dianteiro + lateral + rotativo)

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

N=size(position,1);
pos_final=zeros(N,3);

% Load leg data
footdata;

for i=1:N
    scu=scuid;
    if (servoact>0)
        % Adapatção da posição solicitada para aplicação aos motores
        pos_final(i,:) = position(i,:).*signal(scu,:)*Nrel + offset(scu,:) + calib(scu,:);
    else
        % Adaptação da posição lido nos motores para a das juntas
        pos_final(i,:) = (position(i,:)-offset(scu,:)-calib(scu,:)).*signal(scu,:)/Nrel;
    end
end
