#ifndef __Find_Boundary_H_
#define __Find_Boundary_H_

#include "Behaviors/BehaviorBase.h"
#include "Events/EventRouter.h"

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

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

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

using namespace DualCoding;

/**
 * Reused the code provided on the course webpage -- lab6
 */

class FindBoundary : public VisualRoutinesStateNode {
public:
	
	class FindPinkLine : public VisualRoutinesStateNode {
	public:

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

		virtual void DoStart() {
			const color_index pink_index = ProjectInterface::getColorIndex("pink");

			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[lineDataType].insert(pink_index);

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

			pilot.executeRequest(this, preq);
		}

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

	private:
		MapBuilderRequest mapreq;
	};

	class HeadForPinkLine : public VisualRoutinesStateNode {
	public:

		HeadForPinkLIne() : VisualRoutinesStateNode("HeadForPinkLine") {}

		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 checkForLine() {
    return find_if<LineData>(localShS).isValid();
  }

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

  FindBoundary(): VisualRoutinesStateNode("FindBoundary") {}

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

#endif