function DM = Sim_Walking(DM, walk)

%--------------------------------------------------------------------------------------
% Inicializaçao de constantes e variaveis
%--------------------------------------------------------------------------------------
 %__ Parametros do robo humanoide -----------------------------------------------------
 DM.pe = DM.pe(DM.fase,:);
 DM.XYZRef=DM.XYZRef(DM.fase,:);
 DM.tetaPe = DM.tetaPe(DM.fase);
 DM.fase = 1;
 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) ------
 XYZRef = [0 0 0];              %-- Coordenadas de referencia -------------------------
 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 
%--------------------------------------------------------------------------------------
DM.q = DM.q(DM.fase,:); %-- Postura vertical => flexao inicial dos joelhos ----
%-----------------------------------------------------------------------------------end
Ai = CinDir20dof( DM );
def = CoordJuntas( Ai,DM );
P = def.P;
%-----------------------------------------------------------------------------------end

%--------------------------------------------------------------------------------------
% MOVETYPE: "Walking" - movimentos para a realizaçao de 1 passo
%--------------------------------------------------------------------------------------            
  %------------------------------------------------------------------------------------
  % Parametros de locomoçao
  %------------------------------------------------------------------------------------        
           Sl = walk.Sl;                    %-- Comprimento do passo ------------------------------
           Hh = walk.Hh;                    %-- Altura da anca e Elevaçao do Pe -------------------
           Yg = walk.Yg;                    %-- Posiçao lateral do centro da anca (valor constante)
           Fc = walk.Fc;                    %-- Maxima Elevaçao do Pe -----------------------------
  %------------------------------------------------------------------------------------
  % Descrição da tarefa no espaço operacional => definir os vectores FROM&MOVETO:
  %   [ xh     yh     zh     xf     yf     zf     FOOT:2     PHASE     Texe ]
  %
  %     - (xh,yh,zh) coordenadas da anca
  %     - (xf,yf,zf) coordenadas do pe livre
  %     - FOOT/2, posiçao intermedia do pe livre zf(T/2) (excepto se 0) 
  %     - PHASE = SS: fase de Single-Support; PHASE = DS: fase de Double-Support
  %     - Texe, tempo de execuçao do movimento (execepto vector FROM)
  %
  %  Nota: perna livre alinhada com a de suporte (separaçao entre pes de L6)
  %------------------------------------------------------------------------------------
   FROM   = [ P(15,1)  P(15,2)  P(15,3)-L(7)/2  P(14,1)  P(14,2)  P(14,3)  P(14,3)  DS ];
   MOVETO = [ -Sl/4    Yg          Hh  ,      -Sl      NaN     0.0  ,      0.0     SS      1.0
               Sl/4    Yg          Hh  ,       Sl      NaN     0.0  ,      Fc      SS      1.0
             3*Sl/4    L(8)-Yg     Hh  ,       Sl      NaN     0.0  ,      0.0     DS      0.5 ];

  %------------------------------------------------------------------------------------
  % Determinaçao do vector das juntas Q,dQ,ddQ ao longo de 1 passo (t=0:Tsim): 
  %     1) Algoritmos de cinematica inversa para perna de suporte e perna livre
  %     2) Definiçao das restriçoes cinematicas: anca vertical, pe horizontal ...
  %------------------------------------------------------------------------------------   
   q = DM.q(1,1:12);
   q_sup = DM.q(1,13:end);
   
   Tsim = 0;
   for k=1:size(MOVETO,1),
       %__ Simulação temporal: vector tempo virtual -----------------------------------
        Ts = MOVETO(k,9)/DIM;   %-- Periodo de amonstragem: atençao aos valores! ------
        tp = 0:Ts:MOVETO(k,9);  %-- Tempo de execuçao: Texe = MOVETO(k,10) ------------
        t(k,:) = tp+Tsim;       %-- Matriz tempo de simulaçao -------------------------
       %__ Vector das juntas: cinematica inversa & planeamento nas juntas -------------
        BuildJointMotion;     
       %__ Tempo de execuçao do movimento global --------------------------------------
        Tsim = Tsim+MOVETO(k,9);   
       %__ Reinicializaçao de variaveis: passo dividido em size(MOVETO,1) fases -------
        FROM = MOVETO(k,:);  %-- Reinicializaçao de variaveis -------------------------
        qi = qf;   
   end            
    
  %------------------------------------------------------------------------------------
  %-- Coordenadas Cartesianas do robo completo: cinematica directa para 1-passo
  %------------------------------------------------------------------------------------
  Q_t(1,:) = [ Q(1,1,1)   Q(2,1,1)   Q(3,1,1)   Q(4,1,1)    Q(5,1,1)   Q(6,1,1) ...
              Q(7,1,1)   Q(8,1,1)   Q(9,1,1)   Q(10,1,1)   Q(11,1,1)  Q(12,1,1) q_sup ];

  for k=1:size(Q,3), %-- Movimento dividido em "i" fases -----------------------------

      Q_t(k+1,:) = [Q(1,end,k)   Q(2,end,k)   Q(3,end,k)   Q(4,end,k)    Q(5,end,k)   Q(6,end,k) ...
                    Q(7,end,k)   Q(8,end,k)   Q(9,end,k)   Q(10,end,k)   Q(11,end,k)  Q(12,end,k) q_sup ];

  end
  
  %------------------------------------------------------------------------------------
  %-- Fase intermédia entre o inicio e o fim do passo (quando o pé livre está no ar
  %------------------------------------------------------------------------------------
      Q_x = [ Q(1,round(end/2),2)   Q(2,round(end/2),2)   Q(3,round(end/2),2)   Q(4,round(end/2),2)    Q(5,round(end/2),2)   Q(6,round(end/2),2) ...
              Q(7,round(end/2),2)   Q(8,round(end/2),2)   Q(9,round(end/2),2)   Q(10,round(end/2),2)   Q(11,round(end/2),2)  Q(12,round(end/2),2) q_sup ];
          
  %------------------------------------------------------------------------------------
  %-- Definição do resto dos parametros de definição do movimento
  %------------------------------------------------------------------------------------
      DM.q = [Q_t(1:2,:); Q_x; Q_t(3:4,:)];
      
      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.XYZRef(end+1:end+4,:) = repmat(DM.XYZRef(end,:), 4, 1);

      DM.pe(end+1:end+4,:) = repmat(DM.pe(end,:), 4, 1);

      DM.tetaPe(end+1:end+4) = repmat(DM.tetaPe(end), 4, 1);

      DM.fase = 5;

      DM.pe(end+1,:) = other(DM.pe(end,:));
      
  for k=1:walk.nPassos-1 % numero de passos
      
        Ai = CinDir20dof( DM ); 
        def = CoordJuntas( Ai,DM );
        DM.XYZRef(end+1,:) = def.P(14,:) - [0 0 0];
        
        
        DM.q(end+1:end+3,:) = [fliplr(DM.q(end-2:end,1:12)) DM.q(end-2:end,13:end)];
        
        DM.XYZRef(end+1:end+2,:) = repmat(DM.XYZRef(end,:),2,1); 
        
        DM.pe(end+1:end+2,:) = repmat(DM.pe(end,:), 2, 1);

        DM.tetaPe(end+1:end+3) = repmat(DM.tetaPe(end), 3, 1);
                        
        DM.fase = DM.fase+3;
        
        DM.pe(end+1,:) = other(DM.pe(end,:));

  end
return

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