#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::localMap) : doStart {
    mapreq.addAllObjectColors(lineDataType);
    mapreq.addAllObjectColors(ellipseDataType);
  };

  $nodeclass 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());
  }

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

}

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

#endif