%-------------------------------------------------------------------------------------
% Script: BuildMotion
% Objectivo: com base na descriçao da tarefa, implementa os algoritmos de cinematica 
%            inversa e planeamento de trajectorias nas juntas
%-------------------------------------------------------------------------------------

   %-- Move k ------------------------------------------------------------------------
    Xhi = FROM(1);       Yhi = FROM(2);       Zhi = FROM(3);        %-- ... from   
    Xhf = MOVETO(k,1);   Yhf = MOVETO(k,2);   Zhf = MOVETO(k,3);    %-- ... to  
    Xfi = FROM(4);       Yfi = FROM(5);       Zfi = FROM(6);        %-- ... from   
    Xff = MOVETO(k,4);   Yff = MOVETO(k,5);   Zff = MOVETO(k,6);    %-- ... to  

   %-- PERNA DE SUPORTE --------------------------------------------------------------
   %-- Cinematica inversa ------------------------------------------------------------
    qf_sup = CinInvHip_Sup( MOVETO(k,1:3),L ); 
   %-- Planeamento de trajectorias: VEL e ACE inicial e final ------------------------
    for n=1:length(qf_sup), VEL(n,:) = [0 0]; end %-- Velocidade inicial e final 
    for n=1:length(qf_sup), ACE(n,:) = [0 0]; end %-- Aceleraçao inicial e final
    [Q_sup,dQ_sup,ddQ_sup] = PlaneamentoJuntas( tp,q(1:6),qf_sup,VEL,ACE );     
   %-- PERNA LIVRE -------------------------------------------------------------------   
   switch MOVETO(k,8),
    case DS, %-- Fases de duplo suporte singular -------------------------------------
       %-- Cinematica inversa --------------------------------------------------------
        qf_liv = CinInvHip_Sup( [ MOVETO(k,1)-Sl L(8)-MOVETO(k,2) Hh ],L );
        %-- ATENÇAO: l6-MOVETO(k-2) porque depois a funçao de CI faz o calculo
        %-- da posiçao do ANG1 com base na extremidade da perna de suporte P8
        % qf_liv(1) = -qf_liv(1); qf_liv(5) = -qf_liv(5);
        for n=1:length(qf_liv), AUX_qf(n) = qf_liv(length(qf_liv)+1-n); end, qf_liv = AUX_qf;
        qf = [ qf_sup qf_liv ]; 
       %-- Planeamento de trajectorias: VEL e ACE inicial e final --------------------
        for n=1:length(qf_liv), VEL(n,:) = [0 0]; end %-- Velocidade inicial e final
        for n=1:length(qf_liv), ACE(n,:) = [0 0]; end %-- Aceleraçao inicial e final
        [Q_liv,dQ_liv,ddQ_liv] = PlaneamentoJuntas( tp,qi(7:12),qf_liv,VEL,ACE ); 
        %size(Q_liv), figure(2), plot(1:25,Q_liv(2,:),'r', 1:25,Q_liv(6,:),'b'), pause
    case SS, %-- Fases de suporte singular -------------------------------------------
    if MOVETO(k,6) == MOVETO(k,7),
       %-- Cinematica inversa --------------------------------------------------------    
        qf_liv = CinInvHip_Liv( MOVETO(k,1:6),L );
        qf = [ qf_sup qf_liv ]; 
       %-- Planeamento de trajectorias: VEL e ACE inicial e final --------------------
        for n=1:length(qf_liv), VEL(n,:) = [0 0]; end %-- Velocidade inicial e final
        for n=1:length(qf_liv), ACE(n,:) = [0 0]; end %-- Aceleraçao inicial e final
        [Q_liv,dQ_liv,ddQ_liv] = PlaneamentoJuntas( tp,q(7:12),qf_liv,VEL,ACE ); 
    else
        %-- Posiçao intermedia -------------------------------------------------------
         t1 = tp(1:DIM/2+1);      Tsim1 = t1(length(t1))-t1(1); %-- Vector Tempo: elevar pe
         t2 = tp(DIM/2+1:DIM+1);  Tsim2 = t2(length(t2))-t2(1); %-- Vector Tempo: baixar pe

        %-- Cinematica inversa -------------------------------------------------------        
         qf_liv1 = CinInvHip_Liv( [ PxyzHip( Q_sup(1:3,DIM/2),L ) (Xff+Xfi)/2 Yff MOVETO(k,7) ],L );
         
         %XYZh_if = [ Xhi Xhf; Yhi Yhf; Zhi Zhf ]
         %XYZf_if = [ Xfi Xff; Yfi Yff; Zfi Zff ]
         %XYZh = PxyzHip( Q_sup(1:3,1+DIM/2),L )
         %XYZf = [ (Xff+Xfi)/2 Yff MOVETO(k,7) ], pause
         
         qf_liv2 = CinInvHip_Liv( MOVETO(k,1:6),L );
         qf = [ qf_sup qf_liv2 ];
        %-- Planeamento de trajectorias: VEL e ACE inicial e final -------------------
         for n=1:length(qf_liv1), 
            % Vm(n) = mean( [ ( qf_liv1(n)-qi(n+6) )/Tsim1 ( qf_liv2(n)-qf_liv1(n) )/Tsim2 ] );
             Vm(n) = mean( ( qf_liv1(n)-qi(n+6) )/Tsim1 + ( qf_liv2(n)-qf_liv1(n) )/Tsim2 );
         end      
         for n=1:length(qf_liv1), VEL(n,:) = [0 Vm(n)]; end %-- Velocidade inicial e final 
         for n=1:length(qf_liv1), ACE(n,:) = [0 0];     end %-- Aceleraçao inicial e final
         [Q_liv1,dQ_liv1,ddQ_liv1] = PlaneamentoJuntas( t1,qi(7:12),qf_liv1,VEL,ACE ); 
         for n=1:length(qf_liv1), VEL(n,:) = [Vm(n) 0]; end %-- Velocidade inicial e final
         for n=1:length(qf_liv1), ACE(n,:) = [0     0]; end %-- Aceleraçao inicial e final         
         [Q_liv2,dQ_liv2,ddQ_liv2] = PlaneamentoJuntas( t2,qf_liv1, qf_liv2,VEL,ACE ); 
        %-- Trajectorias globais -----------------------------------------------------
           Q_liv = [   Q_liv1   Q_liv2(:,2:length(  Q_liv2))];
          dQ_liv = [  dQ_liv1  dQ_liv2(:,2:length( dQ_liv2))];
         ddQ_liv = [ ddQ_liv1 ddQ_liv2(:,2:length(ddQ_liv2))];
    end   
   end
  %-- Vector das juntas ---------------------------------------------------------------
   Q(:,:,k) = [ Q_sup ; Q_liv ]; %-- dQ = [ dQ_sup ; dQ_liv ]; ddQ = [ddQ_sup ; ddQ_liv ];

