function DM = Sim_Rotate(DM, rotate)

% keyboard
%--------------------------------------------------------------------------------------
% Inicializaçao de constantes e variaveis
%--------------------------------------------------------------------------------------
 L = DM.L;
 pe = ['dir'; 'esq'];
 %__ Aceleraçao da gravidade ----------------------------------------------------------
 go=9.8;
 %__ Outras constantes ---------------------------------------------------------------- 
 DIM = 24;                      %-- Dimensão do vector tempo de simulaçao (Tsim) ------
 SS = 1; DS = 0;                %-- Definiçao das fases de SS e DS --------------------
 RL = 1; LL = 0;                      %-- Perna de referencia -------------------------------

%-----------------------------------------------------------------------------------end

%--------------------------------------------------------------------------------------
% Representação gráfica do robô na configuração inicial: POSIÇAO ZERO 
%--------------------------------------------------------------------------------------
qi = DM.q(DM.fase,1:20); %-- Postura vertical => flexao inicial dos joelhos ----
%-----------------------------------------------------------------------------------end
Ai = CinDir20dof( DM );
def = CoordJuntas( Ai,DM );
P = def.P;

%__ Parametros do robo humanoide -----------------------------------------------------
 DM.pe = DM.pe(DM.fase,:);
 DM.XYZRef=DM.XYZRef(DM.fase,:);
 DM.tetaPe = DM.tetaPe(DM.fase);
 
% MOVETYPE: Rodar sobre si proprio -> descriçao da tarefa no espaço das juntas
% Parametros de entrada: ang_i, angulo de inclinaçao laeral da perna direita
%                        ang_r, angulo de rotaçao do pe esquerdo
%                        Hh, altura da anca depende de ang_i, mas devo dobrar joelhos!
%--------------------------------------------------------------------------------------
  ang_iR = pi/8;                ang_r  = deg2rad(rotate.graus); %pi/10;          
%-- Valores maximos de Hh e D2, para dado ang_iR, com a perna totalmente esticada -----
  Lt    = L(2)+L(3)+L(4)+L(5);  Hh_max = Lt*cos(ang_iR);  D2_max = Lt*sin(ang_iR);
%-- Especificar altura da anca desejada (dobrar joelhos) e elevaçao do pe -------------
  Hh    = 0.90*Hh_max;          Fc     = 0.0375;
%--------------------------------------------------------------------------------------  
% Partindo da posiçao zero (q = 0):
% 1) Baixar a anca para Hh (i.e., dobrar os joelhos) na vertical
%--------------------------------------------------------------------------------------   
  [q2,q3] = CinInv2R( [Hh/cos(ang_iR)-L(2)-L(5) 0],[L(3) L(4)],[1 -1] );
  qf = [ 0  q2  q3  -q2-q3  0  0  ,  0  0  -q2-q3  q3  q2  0  ,  0 0 0 0 0 0 0 0 ];
%--------------------------------------------------------------------------------------  
% 2) Transferir o CoGx,y para o pe direito: movimento lateral de ang_iR
%--------------------------------------------------------------------------------------  
  qf = [ qf ; ...
         ang_iR  q2  q3  -q2-q3  -ang_iR  0  ,  0  ang_iR  -q2-q3  q3  q2  -ang_iR  ,  0 0 0 0 0 0 0 0 ];
%--------------------------------------------------------------------------------------  
% 3) Elevar o pe esquerdo (Fc em ZZ) ao longo do plano definido pela perna
%--------------------------------------------------------------------------------------  
  [q9,q10] = CinInv2R( [(Hh-Fc)/cos(ang_iR)-L(2)-L(5) 0],[L(4) L(3)],[-1 1] ); q9 = -q9; q10 = -q10;
  qf = [ qf ; ...
         ang_iR  q2  q3  -q2-q3  -ang_iR  0  ,  0  ang_iR  q9  q10  -q9-q10  -ang_iR  ,  0 0 0 0 0 0 0 0 ];
%--------------------------------------------------------------------------------------  
% 4) Rodar o pe esquerdo de um angulo ang_r em torno de eixo vertical (q7)
%--------------------------------------------------------------------------------------  
%  qf = [ qf ; ...
%         ang_iR  q2  q3  -q2-q3  -ang_iR  0  ,  -ang_r  ang_iR  q9  q10  -q9-q10  -ang_iR  ,  0 0 0 0 0 0 0 0 ];
%--------------------------------------------------------------------------------------  
% 5) Alinhar a perna esquerda com o seu novo plano: ang_iL > ang_iR
% ATENÇAO: sera possivel juntar os passos 4 e 5 num unico passo!
%--------------------------------------------------------------------------------------  
  D2 = Hh*tan(ang_iR);  D1 = L(8)-D2; %-- Distancias entre cada pe e a anca esquerda
  L_aux = -D1*cos(ang_r)+sqrt(L(8)^2-D1^2*sin(ang_r)^2); %-- Distancia anca-pe esquerdos 
  if L_aux > D2_max, errordlg('Error: redefine Hh','Error message'), end

  ang_iL = atan2(L_aux,Hh); %-- ANG_iRL = [ ang_iR ang_iL]*180/pi
  qf = [ qf ; ...
         ang_iR  q2  q3  -q2-q3  -ang_iR  0  ,  -ang_r  ang_iL  q9  q10  -q9-q10  -ang_iL  ,  0 0 0 0 0 0 0 0 ];
%--------------------------------------------------------------------------------------  
% 6) Baixar a perna esquerda ao longo do seu novo plano: ang_iL
%    -> confirmar que as Xf,Yf coincidem com as coordenadas X14,Y14
%-------------------------------------------------------------------------------------- 
  [q9,q10] = CinInv2R( [L_aux/sin(ang_iL )-L(2)-L(5) 0],[L(4) L(3)],[-1 1] ); q9 = -q9; q10 = -q10;
  qf = [ qf ; ...
         ang_iR  q2  q3  -q2-q3  -ang_iR  0  ,  -ang_r  ang_iL  q9  q10  -q9-q10  -ang_iL  ,  0 0 0 0 0 0 0 0 ];
%--------------------------------------------------------------------------------------  
% 7) Alinhar a secçao da anca com os pes 
%--------------------------------------------------------------------------------------  
  ang_t = atan2( L_aux*sin(ang_r),D1+L_aux*cos(ang_r) );
  dL = L_aux/sin(ang_iL )-Hh/cos( ang_iR );
  [q9,q10] = CinInv2R( [Hh/cos( ang_iR )-L(2)-L(5) 0],[L(4) L(3)],[-1 1] ); q9 = -q9; q10 = -q10;
 % qf = [ qf ; ...
 %        0  q2  q3  -q2-q3  0  -ang_t  ,  -ang_r+ang_t  0  q9  q10  -q9-q10  0  ,  0 0 0 0 0 0 0 0 ];
%--------------------------------------------------------------------------------------  
% 8) Transferir o CoGx,y para o pe esquerdo: movimento lateral novo
% ATENÇAO: sera possivel juntar os passos 7 e 8 num unico passo!
%--------------------------------------------------------------------------------------  
  ang_iR = atan2(D1,Hh);    Lt_R = D1/sin( ang_iR );
  [q2,q3] = CinInv2R( [Lt_R-L(2)-L(5) 0],[L(3) L(4)],[-1 1] ); q2 = -q2; q3 = -q3;
  ang_iL = atan2(L(8)-L_aux,Hh);    Lt_L = (L(8)-L_aux)/sin( ang_iL ); 
  [q9,q10] = CinInv2R( [Lt_L-L(2)-L(5) 0],[L(4) L(3)],[-1 1] ); q9 = -q9; q10 = -q10;
  qf = [ qf ; ...
         -ang_iR  q2  q3  -q2-q3  ang_iR  -ang_r  ,  0  -ang_iL  q9  q10  -q9-q10  ang_iL  ,  0 0 0 0 0 0 0 0 ];
%--------------------------------------------------------------------------------------  
% 9) Alterar o pé de suporte
%--------------------------------------------------------------------------------------  
    DM.q       = [  qi
                    qf ];
                
    if(DM.pe == 'esq'),
        for k=1:size(DM.q,1),
            DM.q(k,1:12) = fliplr(DM.q(k,1:12));
        end
    end
    
    DM.q(:,end:end+2) = 0;
    DM.pe      = repmat(       DM.pe, size(DM.q,1), 1);
    DM.tetaPe  = repmat(   DM.tetaPe, 1,size(DM.q,1));
    DM.XYZRef  = repmat(   DM.XYZRef, size(DM.q,1), 1);
    
    DM.fase = size(DM.q,1);
    A = CinDir20dof(DM);
    handles = CoordJuntas( A,DM );
    
    DM.q(end+1,:)       = DM.q(end,:);
    if(DM.pe(1,:) == 'esq'),
        DM.tetaPe(end+1)    = DM.tetaPe(end)+ang_r; 
    else
        DM.tetaPe(end+1)    = DM.tetaPe(end)-ang_r; 
    end
    DM.pe(end+1,:)      = other(DM.pe(end,:));
    DM.XYZRef(end+1,:)  = handles.P(14,:) - [0 0 0];
%--------------------------------------------------------------------------------------  
% 10) Elevar o novo pé livre
%--------------------------------------------------------------------------------------  
    DM.q(end+1,:)       = DM.q(end,:);
    DM.q(end,3)         = DM.q(4,10);
    DM.q(end,2)         = DM.q(4,11);
    
    DM.tetaPe(1,end+1)  = DM.tetaPe(1,end); 
    DM.pe(end+1,:)      = DM.pe(end,:);
    DM.XYZRef(end+1,:)  = DM.XYZRef(end,:);
%--------------------------------------------------------------------------------------  
% 11) Rodar o pé livre
%--------------------------------------------------------------------------------------  
    DM.q(end+1,:)       = DM.q(end,:);
    if(DM.pe(1,:) == 'esq'),
        DM.q(end,7)         = 0;
    else
        DM.q(end,6)         = 0;
    end
    
    DM.tetaPe(1,end+1)  = DM.tetaPe(1,end); 
    DM.pe(end+1,:)      = DM.pe(end,:);
    DM.XYZRef(end+1,:)  = DM.XYZRef(end,:);
%--------------------------------------------------------------------------------------  
% 12) Assentar o pé livre
%--------------------------------------------------------------------------------------  
    DM.q(end+1,:)       = DM.q(end,:);
    DM.q(end,1:3)       = DM.q(end-3,1:3);
    
    DM.tetaPe(1,end+1)  = DM.tetaPe(1,end); 
    DM.pe(end+1,:)      = DM.pe(end,:);
    DM.XYZRef(end+1,:)  = DM.XYZRef(end,:);


%--------------------------------------------------------------------------------------  
% Tarefa a Executar: "Rodar-Sobre-Si-Proprio"
%--------------------------------------------------------------------------------------  
DM.fase = 1;
    

  %------------------------------------------------------------------------------------
  % Evolução temporal das juntas ao longo das varias fases
  %------------------------------------------------------------------------------------
  % PlotJuntas( t,Q )
  % PlotCCarts( t,P_x,P_y,P_z )
  % PlotCOG( t,COG )
   
%    for i=1:size(Q,3), %-- Movimento dividido em "i" fases -----------------------------
%              P1  = [ P_x(1,size(Q,2),i), P_y(1,size(Q,2),i), P_z(1,size(Q,2),i) ]
%              P7 = [ P_x(7,size(Q,2),i), P_y(7,size(Q,2),i), P_z(7,size(Q,2),i) ]
%              P8 = [ P_x(8,size(Q,2),i), P_y(8,size(Q,2),i), P_z(8,size(Q,2),i) ]
%              P9 = [ P_x(9,size(Q,2),i), P_y(9,size(Q,2),i), P_z(9,size(Q,2),i) ]
%              P13 = [ P_x(13,size(Q,2),i), P_y(13,size(Q,2),i), P_z(13,size(Q,2),i) ]
%              P14 = [ P_x(14,size(Q,2),i), P_y(14,size(Q,2),i), P_z(14,size(Q,2),i), max(abs( P_z(14,:,i) )) ]
%              Lt_RL  = [P_z(6,size(Q,2),i)-P_z(2,size(Q,2),i) P_z(9,size(Q,2),i)-P_z(13,size(Q,2),i)]
%              
%              pause
%    end  

return


function pe = other(pe)
    switch pe
        case 'dir',
            pe = 'esq';
        case 'esq',
            pe = 'dir';
        otherwise
            disp('pe inválido');
            return
    end
return

