#include "Shared/RobotInfo.h"
#include "Behaviors/StateMachine.h"

$nodeclass Lab5p3 : VisualRoutinesStateNode {

  $nodeclass BuildMap : MapBuilderNode($,MapBuilderRequest::worldMap) : doStart {
	mapreq.addAllObjectColors(lineDataType);
  }

  $nodeclass ExamineMap : SpeechNode : doStart {
    NEW_SHAPEVEC(myline, LineData, select_type<LineData>(worldShS));
    int const numLines = myline.size();
    if ( numLines == 0 )
      textstream << "I did not see any lines.  Try again.";
    //Now given each line, construct ellipses at endpoints
      rgb ecol = myline[0]->getColor();
      myline[0]->setColor(rgb(~ecol.red, ~ecol.green, ~ecol.blue));
      //center points for ellipses
      Point pt1 = myline[0]->end1Pt();
      Point pt2 = myline[0]->end2Pt();
      //Draw ellipses
      NEW_SHAPE(ellipse1, EllipseData, new EllipseData(worldShS, pt1));
      NEW_SHAPE(ellipse2, EllipseData, new EllipseData(worldShS, pt2));
      VRmixin::drawShapes.push_back(ellipse1);
      VRmixin::drawShapes.push_back(ellipse2);
      VRmixin::drawShapes.push_back(myline[0]);
  }

  $setupmachine{
    BuildMap =MAP=> ExamineMap
  }

}

REGISTER_BEHAVIOR(Lab5p3);