#ifndef _Lab6a_h_
#define _Lab6a_h_

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

using namespace std;
using namespace DualCoding;

class Lab6a : public VisualRoutinesStateNode {
public:

	// ******** Component Node Definitions ********

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

		virtual void DoStart() {
			// Build a world map from what we can see
			localShS.clear();  // needed because mapreq.clearShapes will be false
			NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, Lookout::groundSearchPoints(), false));

			worldShS.clear();

			MapBuilderRequest mapreq(MapBuilderRequest::worldMap);
			mapreq.searchArea = gazePoly;
			mapreq.motionSettleTime = 2000;
			mapreq.maxDist = 2000;
			mapreq.clearShapes = false;   // preserve the scan PolygonShape
			mapreq.objectColors[ellipseDataType].insert(pink_index);
			mapreq.objectColors[ellipseDataType].insert(orange_index);
			mapreq.objectColors[ellipseDataType].insert(green_index);
			mapreq.objectColors[ellipseDataType].insert(blue_index);
			mapreq.rawY = true;

			mapBuilder.executeRequest(this,mapreq);
		}

	};

	class Pause : public SoundNode {
	public:
		Pause() : SoundNode("Pause","barklow.wav") {}
		virtual void DoStart() {
			cout << "Built world map.  Reposition the robot and send a blank textmessage to proceed." << endl;
		}
	};

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

		virtual void DoStart() {
			localShS.clear();  // needed because mapreq.clearShapes will be false
			NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, Lookout::groundSearchPoints(), false));

			MapBuilderRequest mapreq(MapBuilderRequest::localMap);
			mapreq.searchArea = gazePoly;
			mapreq.motionSettleTime = 2000;
			mapreq.maxDist = 2000;
			mapreq.clearShapes = false;   // preserve the scan PolygonShape
			mapreq.objectColors[ellipseDataType].insert(pink_index);
			mapreq.objectColors[ellipseDataType].insert(orange_index);
			mapreq.objectColors[ellipseDataType].insert(green_index);
			mapreq.objectColors[ellipseDataType].insert(blue_index);
			mapreq.rawY = true;

			mapBuilder.executeRequest(this,mapreq);
		}

	};

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

		virtual void DoStart() {
			particleFilter->setMinAcceptableWeight(-3);
			for (int i=0; i<20; i++)
				particleFilter->update();
			
			particleFilter->setAgent();
			particleFilter->displayParticles();

			cout << "Done!" << endl;
		}
	};

	// ******** Parent State Machine ********

	static color_index pink_index, orange_index, green_index, blue_index;

  Lab6a() : VisualRoutinesStateNode("Lab6a") {
		// These color indices can't be static consts because we have to
		// wait for the vision pipeline to start up and load the color
		// table before we can call getColorIndex.  Therefore we do the
		// initialization here, in the constructor.
		pink_index = ProjectInterface::getColorIndex("pink");
		orange_index = ProjectInterface::getColorIndex("orange");
		green_index = ProjectInterface::getColorIndex("green");
		blue_index = ProjectInterface::getColorIndex("blue");
	}

	virtual void setup() {
#statemachine
  startnode: 
 		BuildWorldMap() =MAP=> Pause() =E(textmsgEGID)=> 
			BuildLocalMap() =MAP=> Localize()
#endstatemachine
			}

};

#endif