#include "Shared/RobotInfo.h"
#ifdef TGT_HAS_CAMERA

#include "Behaviors/StateMachine.h"
#include "Sound/SoundManager.h"
#include <sstream>

using namespace DualCoding;

$nodeclass SeeShapes : VisualRoutinesStateNode {

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

  $nodeclass ExamineMap : SpeechNode : doStart {
    NEW_SHAPEVEC(lines, LineData, select_type<LineData>(worldShS));
    NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(worldShS));
    int const numLines = lines.size();
    int const numEllipses = ellipses.size();
    if ( numLines + numEllipses == 0 )
      textstream << "I did not see any valid shapes.";
    else {
      textstream << "I saw ";
      if ( numLines == 0 )
	textstream << "no";
      else
	textstream << numLines;
      textstream << " line";
      textstream <<  ( numLines != 1 ? "s" : "");
      textstream << " and ";
      if ( numEllipses == 0 )
	textstream << "no";
      else
	textstream << numEllipses;
      textstream << " ellipse";
      textstream <<  ( numEllipses != 1 ? "s" : "");
    }
  }

  virtual void setup() {
    $statemachine{
      BuildMap =MAP=> ExamineMap
    }
  }

}

REGISTER_BEHAVIOR_MENU(SeeShapes,DEFAULT_TK_MENU"/Vision Demos");

#endif