% APRENDIZAJE AUTOMÁTICO (G. Pajares y J. M. de la Cruz, Eds.)
% Editorial: RA-MA
% Capítulo 12: Aprendizaje por Refuerzo
% Autores: Jose Antonio Martin H y Matilde Santos
function plot_Cart_Pole(s,action,steps)
global f1
global continue_test

if continue_test==false
    return
end

x     = s(1);
theta = s(3);
l=3;
l=3 -sin(abs(theta));     %pole's Length for ploting it can be different from the actual length

pxg = [x+1 x-1 x-1 x+1 x+1];
pyg = [0.25 0.25 1.25 1.25 0.25];

pxp=[x x+l*sin(theta)];
pyp=[1.25 1.25+l*cos(theta)];

arrowfactor_x=sign(action)*2.5;
if (sign(arrowfactor_x)>0)
    text_arrow = strcat('==>> ',int2str(10*action));
else if (sign(arrowfactor_x)<0)
        text_arrow = strcat(int2str(10*action),' <<==');
    else
        text_arrow='=0=';
        arrowfactor_x=0.25;
    end
end

 if isempty(f1)
        f1 = subplot(2,1,1);          
 end 
 
axes(f1);

if continue_test==false
    return
end
%Car 
fill(pxg,pyg,[.6 .6 .5],'LineWidth',2);  %car
hold on
title(f1,['Steps: ',int2str(steps)])

%Car Wheels
plot(f1,x-0.5,0.25,'rO','LineWidth',2,'Markersize',20,'MarkerEdgeColor','k','MarkerFaceColor',[0.5 0.5 0.5]);
plot(f1,x+0.5,0.25,'rO','LineWidth',2,'Markersize',20,'MarkerEdgeColor','k','MarkerFaceColor',[0.5 0.5 0.5]);

%Pendulum
plot(f1,pxp,pyp,'-r','LineWidth',3);
plot(f1,pxp(1),pyp(1),'.r','LineWidth',2,'Markersize',25,'MarkerEdgeColor','k','MarkerFaceColor','r');
plot(f1,pxp(2),pyp(2),'rO','LineWidth',1,'Markersize',15,'MarkerEdgeColor','k','MarkerFaceColor','r');

text(x + arrowfactor_x - 0.5 ,0.8,text_arrow);
%axis([x-6 x+6 0 6])
axis(f1,[-4.5 4.5 0 6])
%grid
%box off
drawnow;
if continue_test==true
    hold off
end
