% Limitação das posições a aplicar a determinado SCU
%
% position_final=limit(scuid,position)
% Entrada:
%   scuid: id do SCU a aplicar as posições
%   position: lista de posições a aplicar às 3 juntas
% Saída:
%   position_final: posição limitada a aplicar às juntas

function position_final=limit(scuid,position)

% Posições mínimas a aplicar a cada SCU
lim_min=[-70 -70 -70;       % SCU 1: Perna direita  (tornozelo + pé + joelho)
         -70 -70 -70;       % SCU 2: Perna esquerda (tornozelo + pé + joelho)
         -70 -70 -70;       % SCU 3: Anca direita   (mov. dianteiro + lateral + rotativo)
         -70 -70 -70];      % SCU 4: Anca esquerdo  (mov. dianteiro + lateral + rotativo)

lim_max=[ 70  70  70;       % SCU 1: Perna direita  (tornozelo + pé + joelho)
          70  70  70;       % SCU 2: Perna esquerda (tornozelo + pé + joelho)
          70  70  70;       % SCU 3: Anca direita   (mov. dianteiro + lateral + rotativo)
          70  70  70];      % SCU 4: Anca esquerdo  (mov. dianteiro + lateral + rotativo)

% Validação dos argumentos
if (scuid<0 || scuid>min(size(lim_min,1),size(lim_max,1)) || length(position)>min(size(lim_min,2),size(lim_max,2)))
    error('limit fcn:invalid arg','Argumentos inválidos!');
end

% Limitação das posições solicitadas
for i=1:length(position)
    if (position(i)<lim_min(scuid,i))
        position(i)=lim_min(scuid,i);
    elseif (position(i)>lim_max(scuid,i))
        position(i)=lim_max(scuid,i);
    end
end

% Retorno
position_final=position;