//-*-c++-*-
#ifndef INCLUDED_Sorter_h_
#define INCLUDED_Sorter_h_

#include "Behaviors/StateMachine.h"
#include "DualCoding/DualCoding.h"
#include "Sound/SoundManager.h"

using namespace DualCoding;

class BuildMap : public VisualRoutinesStateNode {
public:
  BuildMap() : VisualRoutinesStateNode("BuildMap") {}
  
  virtual void DoStart() {
    
    //**************** Set up MapBuilder request ****************
		const int pink_index = ProjectInterface::getColorIndex("pink");
    const int orange_index = ProjectInterface::getColorIndex("orange");
    const int green_index = ProjectInterface::getColorIndex("green");
    const int yellow_index = ProjectInterface::getColorIndex("yellow");
    const int blue_index = ProjectInterface::getColorIndex("blue");

    NEW_SHAPE(gazePt, PointData, new PointData(localShS,Point(400,0,-100,egocentric)));

    MapBuilderRequest req(MapBuilderRequest::localMap);
    req.searchArea = gazePt;
    req.maxDist = 1500;
    req.pursueShapes = true;
		req.numSamples = 5;
		req.motionSettleTime = 2000;

    req.objectColors[lineDataType].insert(blue_index);
    req.objectColors[lineDataType].insert(pink_index);

    req.occluderColors[lineDataType].insert(blue_index);
    req.occluderColors[lineDataType].insert(pink_index);

    req.objectColors[ellipseDataType].insert(blue_index);
    req.objectColors[ellipseDataType].insert(pink_index);

    mapBuilder.executeRequest(this,req);
  }
};

class ExamineMap : public VisualRoutinesStateNode {
public:
	ExamineMap() : VisualRoutinesStateNode("ExamineMap") {}

  virtual void DoStart() {
    sndman->playFile("barklow.wav");
    cout << "MapBuilder found " << localShS.allShapes().size() << " shapes." << endl;
  }
};

class Sorter : public VisualRoutinesStateNode {
public:
  Sorter(): VisualRoutinesStateNode("Sorter") {}
  
	virtual void setup() {
#statemachine
    startnode: ArmNode(){getMC()->moveToPoint(-200,0,0)
	//startnode: BuildMap() =MAP=> ExamineMap()
#endstatemachine
	    }
};

#endif