function Xd=simula(t,X)
  x=X(:,1);dx=X(:,2:$)
  if dx<>[] then
    [xd,dxd]=full_modela(x,dx)
    Xd=[xd,dxd]
  else
    Xd=full_model(X)
  end
endfunction

function [xd,dxd]=full_modela(x,dx)
  z=x(1);th=x(2);zd=x(3);thd=x(4);
  Mati=inv([M+m,m*l*cos(th);cos(th),l]);
  ydd=Mati*[m*l*sin(th)*thd^2+U;g*sin(th)];
  zdd=ydd(1);thdd=ydd(2);
  xd=[zd;thd;zdd;thdd];
  if argn(2)==2 then
    F=[0,m*l*thdd*sin(th)+m*l*thd^2*cos(th);0,zdd*sin(th)+g*cos(th)];
    G=[0,2*m*l*thd*sin(th);0,0];
    dxd=[zeros(2,2),eye(2,2);Mati*F,Mati*G]*dx;
  end
endfunction

function pendanim(anim)
  xset('window',1);xbasc()
  if ~anim then
    driver('Pos')
    xinit('pendfig')
  end
  tc=0;
  XX=[];TT=[];UU=[];Ts=0
  xi=x0;
  for i=1:N
    U=(-1)^(i-1)*Umax;UU=[UU,U];Ts=[Ts,Ts($)+Ti(i)];
    if Ti(i)>1d-9 then	
      tt=tc:.001:tc+Ti(i);
      X=ode(xi,tc,tt(2:$),1d-11,1d-12,simula);
      TT=[TT,tt(2:$)];XX=[XX,X];
      tc=tc+Ti(i);
      xi=X(:,$);
    end
  end
  subplot(4,1,1)
  plot2d(TT',XX(1,:)')
  xgrid()
  subplot(4,1,2)
  plot2d(TT',XX(2,:)')
  xgrid()
  subplot(4,1,3)
  UU($+1)=UU($);
  plot2d2(Ts',UU')
  xgrid()
  subplot(4,1,4)
  plot2d(TT',XX([3,4],:)')
  xgrid()

  if ~anim then
    xend()
  end
  xset('window',2)
  xbasc()

  if ~anim then
    driver('Pos')
    xinit('pendfig2')
  end
  h=gcf();
  nc=100;
  if ~anim then
    h.color_map=graycolormap(nc);
  end
  h.children.isoview='on';
  h.children.data_bounds=[-l,-l;l,l];
  xpoly([-2*l,2*l],[0,0])
  //xpoly([0,0],[-2*l,2*l])
  
  X=XX(:,1);
  z=X(1);th=X(2);
  xp=[z,z+l*sin(th)];yp=[0,l*cos(th)];
  e=l/10;
  xe=z+[-e,-e,e,e,e];ye=[-e,e,e,-e,-e];
  if anim then
    xfpoly(xe,ye);cart=gce();
    xpoly(xp,yp);lnk=gce();
    lnk.thickness = 5;
    lnk.foreground= 5;
  end
  
  if anim then
    for X=XX
      z=X(1);th=X(2);
      xe=z+[-e,-e,e,e,e];
      xp=[z,z+l*sin(th)];yp=[0,l*cos(th)];
      cart.data=[xe',ye'];
      lnk.data=[xp;yp]';
      xpause(10000)
    end
  else
    i=nc
    for t=linspace(0,TT($),nc-1)
      i=i-1;
      j=min(find(t<=TT));
      X=XX(:,j);
      z=X(1);th=X(2);
      xp=[z,z+l*sin(th)];yp=[0,l*cos(th)];
      xpoly(xp,yp);lnk=gce();
      lnk.foreground=  i;
    end
    xend()
    driver('Rec')
    if MSDOS then
      comm=pathconvert(SCI+'\bin\BEpsf',%f,%f,'w')
      unix_g(''"'+comm+''" '+'pendfig')
      unix_g(''"'+comm+''" '+'pendfig2')
    else
      unix_g(SCI+'/bin/BEpsf '+'pendfig')
      unix_g(SCI+'/bin/BEpsf '+'pendfig2')
    end
  end
endfunction