function Juntas=PC_Build_Humanoid_Complete_Inv_Kin(Degrau)
%% Esta funcao compila toda a cinemática inversa do robot para subir o degrau
% INPUT
% Degrau - estrutura com os dados dimensionais do degrau
% OUTPUT
% Juntas - estrutura com os vectores de posição das juntas

dist_referenc_tornozelo=94.28; %mm
% --- Etapas ---
numETAPAS=5;
% ++++++++++++++++++++++++++++++++++
% +++++ Posicionamento Inicial +++++
% ++++++++++++++++++++++++++++++++++
%       ---> Perna Apoio <---
% J1 (Flexão/Extensão Tornozelo)
Juntas.Perna_Apoio.ang_juntas(1,:)=zeros(1,numETAPAS); % graus
% J2 (não actuada neste caso) (Eversão/Inversão Tornozelo)
Juntas.Perna_Apoio.ang_juntas(2,:)=zeros(1,numETAPAS); % graus
% J3(Flexão/Extensão Joelho)
Juntas.Perna_Apoio.ang_juntas(3,:)=zeros(1,numETAPAS); % graus
% J4 (Flexão/Extensão Coxa)
Juntas.Perna_Apoio.ang_juntas(4,:)=zeros(1,numETAPAS); % graus
% J5 (não actuada neste caso) (Rotação Coxa)
Juntas.Perna_Apoio.ang_juntas(5,:)=zeros(1,numETAPAS); % graus
% J6 (não actuada neste caso) (Abdução/Adução Coxa)
Juntas.Perna_Apoio.ang_juntas(6,:)=zeros(1,numETAPAS); % graus

%       ---> Perna Livre <---
% J1 (Flexão/Extensão Tornozelo)
Juntas.Perna_Livre.ang_juntas(1,:)=zeros(1,numETAPAS); % graus
% J2 (não actuada neste caso) (Eversão/Inversão Tornozelo)
Juntas.Perna_Livre.ang_juntas(2,:)=zeros(1,numETAPAS); % graus
% J3(Flexão/Extensão Joelho)
Juntas.Perna_Livre.ang_juntas(3,:)=zeros(1,numETAPAS); % graus
% vJ4 (função que calcula theta4 para o agachamento) (Flexão/Extensão Coxa)
Juntas.Perna_Livre.ang_juntas(4,:)=zeros(1,numETAPAS); % graus
% J5 (não actuada neste caso) (Rotação Coxa)
Juntas.Perna_Livre.ang_juntas(5,:)=zeros(1,numETAPAS); % graus
% J6 (não actuada neste caso) (Abdução/Adução Coxa)
Juntas.Perna_Livre.ang_juntas(6,:)=zeros(1,numETAPAS); % graus

% +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
% +++++ Agachamento de ambas as pernas (descida de 10 mm) +++++
% +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
descida_pelvis=-30; %mm
recuo_pelvis=-30; %mm
[ANCA JOELHO TORNOZELO]=PC_Inv_Kin_Perna_Apoio(recuo_pelvis,descida_pelvis,0);
etapa=2;
%       ---> Perna Apoio <---
% J1 (Flexão/Extensão Tornozelo)
Juntas.Perna_Apoio.ang_juntas(1,etapa)=TORNOZELO; % graus
% J3 (Flexão/Extensão Joelho)
% Juntas.Perna_Apoio.ang_juntas(3,:)=...
%     -2*asind((Dimensoes.Pernas.Perna.comprimento/Dimensoes.Pernas.Coxa.comprimento)*sind(Juntas.Perna_Apoio.ang_juntas(1,:))); % graus
Juntas.Perna_Apoio.ang_juntas(3,etapa)=JOELHO; % graus
% J4 (Flexão/Extensão Coxa)
% Juntas.Perna_Apoio.ang_juntas(4,:)=...
%     asind((Dimensoes.Pernas.Perna.comprimento/Dimensoes.Pernas.Coxa.comprimento)*sind(Juntas.Perna_Apoio.ang_juntas(1,:))); % graus
Juntas.Perna_Apoio.ang_juntas(4,etapa)=ANCA; % graus

%       ---> Perna Livre <---
% J1 (Flexão/Extensão Tornozelo)
Juntas.Perna_Livre.ang_juntas(1,etapa)=-TORNOZELO; % graus
% J3 (Flexão/Extensão Joelho)
Juntas.Perna_Livre.ang_juntas(3,etapa)=-JOELHO; % graus
% J4 (Flexão/Extensão Coxa)
Juntas.Perna_Livre.ang_juntas(4,etapa)=-ANCA; % graus

% +++++++++++++++++++++++++++++++++++++++++++++++++++++
% +++++ "Swing" da perna livre + Avanco do tronco +++++
% +++++++++++++++++++++++++++++++++++++++++++++++++++++
avanco_pelvis=((80+30)/3)-30; %mm
descida_pelvis=-10; %mm
[ANCA JOELHO TORNOZELO]=PC_Inv_Kin_Perna_Apoio(avanco_pelvis,descida_pelvis,0);
etapa=etapa+1;
%       ---> Perna Apoio <---
% J1 (Flexão/Extensão Tornozelo)
Juntas.Perna_Apoio.ang_juntas(1,etapa)=TORNOZELO; % graus
% J3 (Flexão/Extensão Joelho)
Juntas.Perna_Apoio.ang_juntas(3,etapa)=JOELHO; % graus
% J4 (Flexão/Extensão Coxa)
Juntas.Perna_Apoio.ang_juntas(4,etapa)=ANCA; % graus

% posicao desejada
X=Degrau.Distancia-avanco_pelvis;%+dist_referenc_tornozelo+55.72;%-avanco_pelvis+; %-+
Y=60+Degrau.Altura-descida_pelvis; %(-10 anteriormente)

[ANCA JOELHO TORNOZELO]=...
    PC_Inv_Kin_Perna_Livre(X,Y,...
        PC_Determine_Best_Phi_Given_Position(X,Y,0.01));
% [ANCA JOELHO TORNOZELO]=PC_Inv_Kin_Perna_Livre(100,29,20);
%       ---> Perna Livre <---
% J1 (Flexão/Extensão Tornozelo)
Juntas.Perna_Livre.ang_juntas(1,etapa)=-TORNOZELO; % graus
% J3 (Flexão/Extensão Joelho)
Juntas.Perna_Livre.ang_juntas(3,etapa)=-JOELHO; % graus
% J4 (Flexão/Extensão Coxa)
Juntas.Perna_Livre.ang_juntas(4,etapa)=-ANCA; % graus

% +++++++++++++++++++++++++++++++++++++++++++++++++++++
% +++++ 2ª Fase de Posicionamento +++++
% +++++++++++++++++++++++++++++++++++++++++++++++++++++
avanco_pelvis=2*((80+30)/3)-30; %mm
descida_pelvis=-8; %mm
[ANCA JOELHO TORNOZELO]=PC_Inv_Kin_Perna_Apoio(avanco_pelvis,descida_pelvis,0);
etapa=etapa+1;
%       ---> Perna Apoio <---
% J1 (Flexão/Extensão Tornozelo)
Juntas.Perna_Apoio.ang_juntas(1,etapa)=TORNOZELO; % graus
% J3 (Flexão/Extensão Joelho)
Juntas.Perna_Apoio.ang_juntas(3,etapa)=JOELHO; % graus
% J4 (Flexão/Extensão Coxa)
Juntas.Perna_Apoio.ang_juntas(4,etapa)=ANCA; % graus

% posicao desejada
X = dist_referenc_tornozelo + Degrau.Distancia + 55.72 - avanco_pelvis;
% Y=step_percent_2*Degrau.Altura-descida_pelvis; %(-10 anteriormente)
if Degrau.Altura>=110
    Y=55+Degrau.Altura-descida_pelvis;
else
    Y=25+Degrau.Altura-descida_pelvis;
end
[ANCA JOELHO TORNOZELO]=...
    PC_Inv_Kin_Perna_Livre(X,Y,...
        PC_Determine_Biggest_Phi_Given_Position(X,Y,0.01));
% [ANCA JOELHO TORNOZELO]=PC_Inv_Kin_Perna_Livre(100,29,20);
%       ---> Perna Livre <---
% J1 (Flexão/Extensão Tornozelo)
Juntas.Perna_Livre.ang_juntas(1,etapa)=-TORNOZELO; % graus
% J3 (Flexão/Extensão Joelho)
Juntas.Perna_Livre.ang_juntas(3,etapa)=-JOELHO; % graus
% J4 (Flexão/Extensão Coxa)
Juntas.Perna_Livre.ang_juntas(4,etapa)=-ANCA; % graus

% +++++++++++++++++++++++++++++++++++++++++++++++
% +++++ Posicionamento final da perna livre +++++
% +++++++++++++++++++++++++++++++++++++++++++++++
avanco_pelvis=80; %mm
descida_pelvis=-12; %mm
[ANCA JOELHO TORNOZELO]=PC_Inv_Kin_Perna_Apoio(avanco_pelvis,descida_pelvis,0);
etapa=etapa+1;
%       ---> Perna Apoio <---
% J1 (Flexão/Extensão Tornozelo)
Juntas.Perna_Apoio.ang_juntas(1,etapa)=TORNOZELO; % graus
% J3 (Flexão/Extensão Joelho)
Juntas.Perna_Apoio.ang_juntas(3,etapa)=JOELHO; % graus
% J4 (Flexão/Extensão Coxa)
Juntas.Perna_Apoio.ang_juntas(4,etapa)=ANCA; % graus

% posicao desejada
X = dist_referenc_tornozelo + Degrau.Distancia + 55.72 - avanco_pelvis;
Y = Degrau.Altura - descida_pelvis;

[ANCA JOELHO TORNOZELO]=...
    PC_Inv_Kin_Perna_Livre(X,Y,...
    PC_Determine_Best_Phi_Given_Position(X,Y,0.01));
%       ---> Perna Livre <---
% J1 (Flexão/Extensão Tornozelo)
Juntas.Perna_Livre.ang_juntas(1,etapa)=-TORNOZELO; % graus
% J3 (Flexão/Extensão Joelho)
Juntas.Perna_Livre.ang_juntas(3,etapa)=-JOELHO; % graus
% J4 (Flexão/Extensão Coxa)
Juntas.Perna_Livre.ang_juntas(4,etapa)=-ANCA; % graus

% ++++++ Resto do corpo ++++++

%       ---> Tronco <---
% J7 (não actuada neste caso) (Rotação Tronco)
Juntas.Tronco.ang_juntas(1,:)=zeros(1,numETAPAS); % graus
% J8 (não actuada neste caso) (Abdução Tronco)
Juntas.Tronco.ang_juntas(2,:)=zeros(1,numETAPAS); % graus
% J9 (não actuada neste caso) (Inclinação Tronco)
Juntas.Tronco.ang_juntas(3,:)=zeros(1,numETAPAS); % graus

%       ---> Cabeca <---
% J10 (não actuada neste caso) (Inclinacao Cabeca)
Juntas.Cabeca.ang_juntas(1,:)=zeros(1,numETAPAS); % graus

%       ---> Bracos <---
% Braco direito
% J11 (não actuada neste caso) (Flexao Braco) (OMBRO)
Juntas.Braco_Direito.ang_juntas(1,:)=zeros(1,numETAPAS); % graus
% J12 (não actuada neste caso) (Abducao Braco)
Juntas.Braco_Direito.ang_juntas(2,:)=zeros(1,numETAPAS); % graus
% J13 (não actuada neste caso) (Rotacao Braco)
Juntas.Braco_Direito.ang_juntas(3,:)=zeros(1,numETAPAS); % graus
% J14 (não actuada neste caso) (Flexao Cotovelo)
Juntas.Braco_Direito.ang_juntas(4,:)=zeros(1,numETAPAS); % graus
% Braco esquerdo
% J11 (não actuada neste caso) (Flexao Braco) (OMBRO)
Juntas.Braco_Esquerdo.ang_juntas(1,:)=zeros(1,numETAPAS); % graus
% J12 (não actuada neste caso) (Abducao Braco)
Juntas.Braco_Esquerdo.ang_juntas(2,:)=zeros(1,numETAPAS); % graus
% J13 (não actuada neste caso) (Rotacao Braco)
Juntas.Braco_Esquerdo.ang_juntas(3,:)=zeros(1,numETAPAS); % graus
% J14 (não actuada neste caso) (Flexao Cotovelo)
Juntas.Braco_Esquerdo.ang_juntas(4,:)=zeros(1,numETAPAS); % graus

end