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

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

using namespace DualCoding;

#nodeclass SeeShapes : VisualRoutinesStateNode

  #shortnodeclass BuildMap : MapBuilderNode($,MapBuilderRequest::localMap) : DoStart
      mapreq.addAllObjectColors(lineDataType);
      mapreq.addAllObjectColors(ellipseDataType);

  #shortnodeclass ExamineMap : VisualRoutinesStateNode : DoStart
      NEW_SHAPEVEC(lines, LineData, select_type<LineData>(localShS));
      NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(localShS));
      int const numLines = lines.size();
      int const numEllipses = ellipses.size();
      ostringstream result;
      if ( numLines + numEllipses == 0 )
	result << "I did not see any valid shapes.";
      else {
	result << "I saw ";
	if ( numLines == 0 )
	  result << "no";
	else
	  result << numLines;
	result << " line";
	result <<  ( numLines != 1 ? "s" : "");
	result << " and ";
	if ( numEllipses == 0 )
	  result << "no";
	else
	  result << numEllipses;
	result << " ellipse";
	result <<  ( numEllipses != 1 ? "s" : "");
	result << ".";
      }
      sndman->speak(result.str());

  #shortnodemethod setup
    #statemachine
      startnode: BuildMap() =MAP=> ExamineMap()
    #endstatemachine

#endnodeclass

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

#endif