% Calcula a cinemática inversa de uma perna, a partir das coordenadas (x,y)
% da base do pé
% As coordenadas (x,y) usam como origem o solo.
% Assim a coordenada x define o deslocamento na horizontal e a
% coordenada y representa a altura do pé relativamente ao solo.

function [theta,error,errorstr]=cinematinv(xyz)

% Carregar variáveis
footdata;

% Inicialização dos parametros de saída
theta=[0 0 0];
error=0;
errorstr='OK';

% Número de linhas de (x,y)
N=size(xyz,1);

% Propriedades da perna
L0=L(1);
L1=L(2);
L2=L(3);
L3=L(4);

% Conversão das coordenadas usando como origem o topo da anca
x=xyz(:,1);             % Coordenada x
y=xyz(:,2);             % Coordenada y
z=xyz(:,3);             % Coordenada z

% ___ PLANO XOZ ____

% Coordenadas entrando só em linha de conta com os elos móveis no plano xoz
xx=x;
yy=y;
zz=z-L0;
d=sqrt(zz.^2+xx.^2);

% Validação das coordenadas xx e zz
if (sum(d>L1+L2+L3)>0)
    error=1;
    errorstr='Coordenadas inválidas!';
    return;
end

% Cálculo da inclinação da perna
theta1=atan2(xx,zz);

% ___ PLANO YOZ ____

% Cálculo do comprimento dos elos projectados sobre o plano yoz.
l0=L0;
l1=L1*cos(theta1);
l2=L2*cos(theta1);
l3=L3*cos(theta1);

% Coordenadas só considerando os elos móveis no plano yoz
xx=x;
yy=y;
zz=z-l1-l0;
d=sqrt(zz.^2+yy.^2);

% Validação das coordenadas yy e zz
if (sum(d>l2+l3)>0)
    error=1;
    errorstr='Coordenadas inválidas!';
    return;
end

% Cálculo de theta
theta=zeros(N,3);
theta(:,1)=theta1;

% Ks para cálculo de theta2
K1=2*zz.*l2;
K2=2*yy.*l2;
K3=d.^2+l2.^2-l3.^2;

theta(:,2)=atan2(K1,K2)-atan2(sqrt(K1.^2+K2.^2-K3.^2),K3)-pi/2;

% Ks para cálculo de theta3
K1=zeros(N,1);
K2=2*l2.*l3;
K3=d.^2-l2.^2-l3.^2;

theta(:,3)=atan2(K1,K2)+atan2(sqrt(K1.^2+K2.^2-K3.^2),K3);

theta=theta*180/pi;