% R_PLOTHIST	Plot the complete process of square box pushing.

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

close all;
clear all;

%FLname='d:\maxplus2\rect\result\rect01';
FLname='d:\maxplus2\rect\result\rect02';

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

BeginPoint=1;
EndPoint=158;

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

StepLib=[84 90 91 102 112 122 132 142 152];
%for mm=1:12
for mm=1:9
  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=StepLib(mm);

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

  plot(70,90, 'Marker','^');
  if Step>90
    plot(39,26, 'Marker','d');
  end;
  plot(CurrentGoal(1),CurrentGoal(2), 'Marker','+');

      [FirstArray,SecondArray,WhichPoint]=r_plotsquare(OHistory(Step,:),ObjectSize);
      line(FirstArray,SecondArray);
      line([OHistory(Step,1) WhichPoint(1)],[OHistory(Step,2) WhichPoint(2)]);

  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\rect\result\ps\r2h' num2str(Step)];
  gtext(Title_Text);
  eval(['print ' SaveString ' -dps;']);
end;