% C_PLOTHIST	Plot the process for round box pushing.

%     Copyright (c) 1998-2000 Jiming Liu and Jianbing Wu

close all;
clear all;

%FLname='d:\maxplus2\circ\result\circ01';
FLname='d:\maxplus2\circ\result\circ02';

eval(['load ' FLname ';']);

BeginPoint=1;
EndPoint=131;

RHistory=RobotHistory(BeginPoint:EndPoint,:);
OHistory=ObjectHistory(BeginPoint:EndPoint,:);
GHistory=GoalHistory(BeginPoint:EndPoint,:);
ChHistory=ChromHistory(BeginPoint:EndPoint,:);

%for mm=1:12
for mm=1:6
  clf;
  plot(0,1,'color',[1 1 1]);
%  axis([10,90,15,95]);
  axis([5,95,5,95]);
  hold on;

%  Step=(mm-1)*6+1;
  Step=(mm-1)*10+81;

  ObjectPosition=OHistory(Step,:);
  CurrentGoal=GHistory(Step,:);
  DeltaPosition=CurrentGoal-ObjectPosition;
  AxisDirection=atan2(DeltaPosition(2),DeltaPosition(1));

  plot(70,90, 'Marker','^');
  plot(25,57, 'Marker','d');
  plot(ObjectPosition(1),ObjectPosition(2),'Marker','o','Color','k','markersize',14);
  plot(CurrentGoal(1),CurrentGoal(2), 'Marker','+');
  for nn=1:3
    DrawDirection=ChHistory(Step,3+nn)*2*pi/32+AxisDirection+pi;
    CurrentRobot=RHistory(Step,(nn-1)*2+1:2*nn);
    [FirstArray,SecondArray,Fill1Array,Fill2Array]=plotrob(ObjectPosition,CurrentRobot,RobotSize,ObjectSize);
    line(FirstArray,SecondArray);
    fill(Fill1Array,Fill2Array,'k');

    DrawPoint=[(FirstArray(1)+FirstArray(3))/2 (SecondArray(1)+SecondArray(3))/2];
    DrawEnd=DrawPoint+[3*cos(DrawDirection) 3*sin(DrawDirection)];
    line([DrawPoint(1) DrawEnd(1)],[DrawPoint(2) DrawEnd(2)]);
  end;
  
  drawnow;
  Title_Text=['(t=' num2str(Step) ')'];
  SaveString=['d:\maxplus2\circ\result\ps\c2h' num2str(Step)];
  gtext(Title_Text);
  eval(['print ' SaveString ' -dps;']);
end;