% 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,error,errorstr]=validate(scuid,position)

error=0;
errorstr='OK';

% Posições limite a aplicar a cada SCU
posmin=[-90 -90 -90;
        -90 -90 -90;
        -90 -90 -90;
        -90 -90 -90];

posmax=[ 90  90  90;
         90  90  90;
         90  90  90;
         90  90  90];

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

N=size(position,1);

error=0;
for k=1:N

    % Verificação de uma situação de limitação
    error=error | (sum((position(k,:)<posmin(scuid,:))+(position(k,:)>posmax(scuid,:)))>0);
    
    % Limitação das posições
    position(k,:)=(position(k,:)<posmin(scuid,:)).*posmin(scuid,:)+ ...
                  (position(k,:)>posmax(scuid,:)).*posmax(scuid,:)+ ...
                  ((position(k,:)>=posmin(scuid,:)) & (position(k,:)<=posmax(scuid,:))).*position(k,:);    
end

if (error==1)
    errorstr='Posições limitadas!';
end