% Cálculo do Centro de pressão nas coordenadas (x,y,z) de uma perna.
% O argumento <react> possui os valores dos sensores de força do pé e
% permitem calcular as componentes (x,y) do Centro de Pressão.
% O argumento <theta> possui os angulos das juntas (ortogonal ao pé,
% paralelo ao pé, joelho) e permite calcular a componente z do CoP.
% A componente z corresponde à altura da perna.
%
% Syntax: CoP=calccop(react,theta)
%
% Argumentos de entrada:
%   react -> Sensores de força do pé
%   theta -> Ângulos das juntas da perna
%
% Saída:
%   CoP -> Centro de Pressão nas componentes (x,y,z).

function CoP=calccop(react,servo,scuid)

if (nargin<3)
    scuid=1;
end

% Características do pé
footdata;

amplifier_factor=1;

delta_reactx=0;
delta_reacty=-50;

N=size(react,1);
CoP=zeros(N,3);

for i=1:N
    % Centro de pressão (x,y) baseado nos sensores de força
    if (sum(react(i,:))~=0)
        CoP(i,1)=sum((react(i,:)+delta_reactx).*sensor_locx)/sum(react(i,:)+delta_reactx)*amplifier_factor;
        CoP(i,2)=sum((react(i,:)+delta_reacty).*sensor_locy)/sum(react(i,:)+delta_reacty)*amplifier_factor;
    end
    % Centro de pressão z baseado nos ângulos das juntas (altura da perna)
    if (nargin>=2)
        theta=convertangles(scuid,servo(i,:),-1)*pi/180;
        CoP(i,3)=leg_low+leg_high-(leg_low*cos(theta(2))+leg_high*cos(theta(2)+theta(3)))*cos(theta(1));
    end
end

% Ajuste do CoP calculado de modo a dar o mesmo valor que o uC PIC
CoP(:,1:2)=CoP(:,1:2)*100;