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

#include "DualCoding/DualCoding.h"
#include "Events/EventBase.h" 

using namespace DualCoding;

class Lab6b : public VisualRoutinesStateNode {
public:
	
	class FindGreenPillar : public VisualRoutinesStateNode {
	public:

		FindGreenPillar() : VisualRoutinesStateNode("FindGreenPillar"), mapreq(MapBuilderRequest::localMap) {}

		virtual void DoStart() {
			const color_index green_index = ProjectInterface::getColorIndex("green");

			localShS.clear();
			NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, Lookout::groundSearchPoints(), false));

			// Prepare a MapBuilder request that will be passed to the Pilot
			mapreq.searchArea = gazePoly;
			mapreq.maxDist = 2000;
			mapreq.clearShapes = false;
			mapreq.rawY = true;
			mapreq.objectColors[blobDataType].insert(green_index);
			mapreq.minBlobAreas[green_index] = 100;
			mapreq.blobOrientations[green_index] = BlobData::pillar;  // this blob stands on the floor; it does not lie in the ground plane

			PilotRequest preq(PilotRequest::visualSearch);
			preq.mapBuilderRequest = &mapreq;
			preq.exitTest = &checkForBlob;
			preq.searchRotationAngle = 1;  // radians

			pilot.executeRequest(this, preq);
		}

		virtual void DoStop() {
			pilot.abort();
		}

	private:
		MapBuilderRequest mapreq;
	};

	class HeadForGreenPillar : public VisualRoutinesStateNode {
	public:

		HeadForGreenPillar() : VisualRoutinesStateNode("HeadForOranveCan") {}

		virtual void DoStart() {
      PilotRequest preq(PilotRequest::gotoShape);
      NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));
      NEW_SHAPE(biggestblob, BlobData, max_element(blobs,BlobData::AreaLessThan()));
      preq.targetShape = biggestblob;
      pilot.executeRequest(this,preq);
    }
  };

  static bool checkForBlob() {
    return find_if<BlobData>(localShS).isValid();
  }

	//******** Parent state machine ********

  Lab6b(): VisualRoutinesStateNode("Lab6b") {}

	virtual void setup() {
#statemachine
	startnode: FindGreenPillar() =PILOT=> HeadForGreenPillar()
#endstatemachine
	}
};

#endif